libaf/af_surround.c

   1 /*
   2  * Filter to do simple decoding of matrixed surround sound.
   3  * This will provide a (basic) surround-sound effect from
   4  * audio encoded for Dolby Surround, Pro Logic etc.
   5  *
   6  * original author: Steve Davies <steve@daviesfam.org>
   7  *
   8  * This file is part of MPlayer.
   9  *
  10  * MPlayer is free software; you can redistribute it and/or modify
  11  * it under the terms of the GNU General Public License as published by
  12  * the Free Software Foundation; either version 2 of the License, or
  13  * (at your option) any later version.
  14  *
  15  * MPlayer is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  *
  20  * You should have received a copy of the GNU General Public License along
  21  * with MPlayer; if not, write to the Free Software Foundation, Inc.,
  22  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  23  */
  24
  25 /* The principle:  Make rear channels by extracting anti-phase data
  26    from the front channels, delay by 20ms and feed to rear in anti-phase
  27 */
  28
  29
  30 /* SPLITREAR: Define to decode two distinct rear channels - this
  31      doesn't work so well in practice because separation in a passive
  32      matrix is not high. C (dialogue) to Ls and Rs 14dB or so - so
  33      dialogue leaks to the rear.  Still - give it a try and send
  34      feedback. Comment this define for old behavior of a single
  35      surround sent to rear in anti-phase */
  36 #define SPLITREAR 1
  37
  38 #include <stdio.h>
  39 #include <stdlib.h>
  40 #include <string.h>
  41
  42 #include "af.h"
  43 #include "dsp.h"
  44
  45 #define L  32    // Length of fir filter
  46 #define LD 65536 // Length of delay buffer
  47
  48 // 32 Tap fir filter loop unrolled
  49 #define FIR(x,w,y) \
  50   y = ( w[0] *x[0] +w[1] *x[1] +w[2] *x[2] +w[3] *x[3]  \
  51       + w[4] *x[4] +w[5] *x[5] +w[6] *x[6] +w[7] *x[7]  \
  52       + w[8] *x[8] +w[9] *x[9] +w[10]*x[10]+w[11]*x[11] \
  53       + w[12]*x[12]+w[13]*x[13]+w[14]*x[14]+w[15]*x[15] \
  54       + w[16]*x[16]+w[17]*x[17]+w[18]*x[18]+w[19]*x[19] \
  55       + w[20]*x[20]+w[21]*x[21]+w[22]*x[22]+w[23]*x[23] \
  56       + w[24]*x[24]+w[25]*x[25]+w[26]*x[26]+w[27]*x[27] \
  57       + w[28]*x[28]+w[29]*x[29]+w[30]*x[30]+w[31]*x[31])
  58
  59 // Add to circular queue macro + update index
  60 #ifdef SPLITREAR
  61 #define ADDQUE(qi,rq,lq,r,l)\
  62   lq[qi]=lq[qi+L]=(l);\
  63   rq[qi]=rq[qi+L]=(r);\
  64   qi=(qi-1)&(L-1);
  65 #else
  66 #define ADDQUE(qi,lq,l)\
  67   lq[qi]=lq[qi+L]=(l);\
  68   qi=(qi-1)&(L-1);
  69 #endif
  70
  71 // Macro for updating queue index in delay queues
  72 #define UPDATEQI(qi) qi=(qi+1)&(LD-1)
  73
  74 // instance data
  75 typedef struct af_surround_s
  76 {
  77   float lq[2*L]; // Circular queue for filtering left rear channel
  78   float rq[2*L]; // Circular queue for filtering right rear channel
  79   float w[L];    // FIR filter coefficients for surround sound 7kHz low-pass
  80   float* dr;     // Delay queue right rear channel
  81   float* dl;     // Delay queue left rear channel
  82   float  d;      // Delay time
  83   int i;         // Position in circular buffer
  84   int wi;        // Write index for delay queue
  85   int ri;        // Read index for delay queue
  86 }af_surround_t;
  87
  88 // Initialization and runtime control
  89 static int control(struct af_instance_s* af, int cmd, void* arg)
  90 {
  91   af_surround_t *s = af->setup;
  92   switch(cmd){
  93   case AF_CONTROL_REINIT:{
  94     float fc;
  95     af->data->rate   = ((af_data_t*)arg)->rate;
  96     af->data->nch    = ((af_data_t*)arg)->nch*2;
  97     af->data->format = AF_FORMAT_FLOAT_NE;
  98     af->data->bps    = 4;
  99
 100     if (af->data->nch != 4){
 101       mp_msg(MSGT_AFILTER, MSGL_ERR, "[surround] Only stereo input is supported.\n");
 102       return AF_DETACH;
 103     }
 104     // Surround filer coefficients
 105     fc = 2.0 * 7000.0/(float)af->data->rate;
 106     if (-1 == af_filter_design_fir(L, s->w, &fc, LP|HAMMING, 0)){
 107       mp_msg(MSGT_AFILTER, MSGL_ERR, "[surround] Unable to design low-pass filter.\n");
 108       return AF_ERROR;
 109     }
 110
 111     // Free previous delay queues
 112     if(s->dl)
 113       free(s->dl);
 114     if(s->dr)
 115       free(s->dr);
 116     // Allocate new delay queues
 117     s->dl = calloc(LD,af->data->bps);
 118     s->dr = calloc(LD,af->data->bps);
 119     if((NULL == s->dl) || (NULL == s->dr))
 120       mp_msg(MSGT_AFILTER, MSGL_FATAL, "[delay] Out of memory\n");
 121
 122     // Initialize delay queue index
 123     if(AF_OK != af_from_ms(1, &s->d, &s->wi, af->data->rate, 0.0, 1000.0))
 124       return AF_ERROR;
 125 //    printf("%i\n",s->wi);
 126     s->ri = 0;
 127
 128     if((af->data->format != ((af_data_t*)arg)->format) ||
 129        (af->data->bps    != ((af_data_t*)arg)->bps)){
 130       ((af_data_t*)arg)->format = af->data->format;
 131       ((af_data_t*)arg)->bps = af->data->bps;
 132       return AF_FALSE;
 133     }
 134     return AF_OK;
 135   }
 136   case AF_CONTROL_COMMAND_LINE:{
 137     float d = 0;
 138     sscanf((char*)arg,"%f",&d);
 139     if ((d < 0) || (d > 1000)){
 140       mp_msg(MSGT_AFILTER, MSGL_ERR, "[surround] Invalid delay time, valid time values"
 141              " are 0ms to 1000ms current value is %0.3f ms\n",d);
 142       return AF_ERROR;
 143     }
 144     s->d = d;
 145     return AF_OK;
 146   }
 147   }
 148   return AF_UNKNOWN;
 149 }
 150
 151 // Deallocate memory
 152 static void uninit(struct af_instance_s* af)
 153 {
 154   if(af->data)
 155     free(af->data->audio);
 156   free(af->data);
 157   free(af->setup);
 158 }
 159
 160 // The beginnings of an active matrix...
 161 static float steering_matrix[][12] = {
 162 //      LL      RL      LR      RR      LS      RS
 163 //      LLs     RLs     LRs     RRs     LC      RC
 164        {.707,   .0,     .0,     .707,   .5,     -.5,
 165         .5878,  -.3928, .3928,  -.5878, .5,     .5},
 166 };
 167
 168 // Experimental moving average dominance
 169 //static int amp_L = 0, amp_R = 0, amp_C = 0, amp_S = 0;
 170
 171 // Filter data through filter
 172 static af_data_t* play(struct af_instance_s* af, af_data_t* data){
 173   af_surround_t* s   = (af_surround_t*)af->setup;
 174   float*         m   = steering_matrix[0];
 175   float*         in  = data->audio;     // Input audio data
 176   float*         out = NULL;            // Output audio data
 177   float*         end = in + data->len / sizeof(float); // Loop end
 178   int            i   = s->i;    // Filter queue index
 179   int            ri  = s->ri;   // Read index for delay queue
 180   int            wi  = s->wi;   // Write index for delay queue
 181
 182   if (AF_OK != RESIZE_LOCAL_BUFFER(af, data))
 183     return NULL;
 184
 185   out = af->data->audio;
 186
 187   while(in < end){
 188     /* Dominance:
 189        abs(in[0])  abs(in[1]);
 190        abs(in[0]+in[1])  abs(in[0]-in[1]);
 191        10 * log( abs(in[0]) / (abs(in[1])|1) );
 192        10 * log( abs(in[0]+in[1]) / (abs(in[0]-in[1])|1) ); */
 193
 194     /* About volume balancing...
 195        Surround encoding does the following:
 196            Lt=L+.707*C+.707*S, Rt=R+.707*C-.707*S
 197        So S should be extracted as:
 198            (Lt-Rt)
 199        But we are splitting the S to two output channels, so we
 200        must take 3dB off as we split it:
 201            Ls=Rs=.707*(Lt-Rt)
 202        Trouble is, Lt could be +1, Rt -1, so possibility that S will
 203        overflow. So to avoid that, we cut L/R by 3dB (*.707), and S by
 204        6dB (/2). This keeps the overall balance, but guarantees no
 205        overflow. */
 206
 207     // Output front left and right
 208     out[0] = m[0]*in[0] + m[1]*in[1];
 209     out[1] = m[2]*in[0] + m[3]*in[1];
 210
 211     // Low-pass output @ 7kHz
 212     FIR((&s->lq[i]), s->w, s->dl[wi]);
 213
 214     // Delay output by d ms
 215     out[2] = s->dl[ri];
 216
 217 #ifdef SPLITREAR
 218     // Low-pass output @ 7kHz
 219     FIR((&s->rq[i]), s->w, s->dr[wi]);
 220
 221     // Delay output by d ms
 222     out[3] = s->dr[ri];
 223 #else
 224     out[3] = -out[2];
 225 #endif
 226
 227     // Update delay queues indexes
 228     UPDATEQI(ri);
 229     UPDATEQI(wi);
 230
 231     // Calculate and save surround in circular queue
 232 #ifdef SPLITREAR
 233     ADDQUE(i, s->rq, s->lq, m[6]*in[0]+m[7]*in[1], m[8]*in[0]+m[9]*in[1]);
 234 #else
 235     ADDQUE(i, s->lq, m[4]*in[0]+m[5]*in[1]);
 236 #endif
 237
 238     // Next sample...
 239     in = &in[data->nch];
 240     out = &out[af->data->nch];
 241   }
 242
 243   // Save indexes
 244   s->i  = i; s->ri = ri; s->wi = wi;
 245
 246   // Set output data
 247   data->audio = af->data->audio;
 248   data->len   *= 2;
 249   data->nch   = af->data->nch;
 250
 251   return data;
 252 }
 253
 254 static int af_open(af_instance_t* af){
 255   af->control=control;
 256   af->uninit=uninit;
 257   af->play=play;
 258   af->mul=2;
 259   af->data=calloc(1,sizeof(af_data_t));
 260   af->setup=calloc(1,sizeof(af_surround_t));
 261   if(af->data == NULL || af->setup == NULL)
 262     return AF_ERROR;
 263   ((af_surround_t*)af->setup)->d = 20;
 264   return AF_OK;
 265 }
 266
 267 af_info_t af_info_surround =
 268 {
 269         "Surround decoder filter",
 270         "surround",
 271         "Steve Davies <steve@daviesfam.org>",
 272         "",
 273         AF_FLAGS_NOT_REENTRANT,
 274         af_open
 275 };