libaf/af_equalizer.c

   1 /*
   2  * Equalizer filter, implementation of a 10 band time domain graphic
   3  * equalizer using IIR filters. The IIR filters are implemented using a
   4  * Direct Form II approach, but has been modified (b1 == 0 always) to
   5  * save computation.
   6  *
   7  * Copyright (C) 2001 Anders Johansson ajh@atri.curtin.edu.au
   8  *
   9  * This file is part of MPlayer.
  10  *
  11  * MPlayer is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License as published by
  13  * the Free Software Foundation; either version 2 of the License, or
  14  * (at your option) any later version.
  15  *
  16  * MPlayer is distributed in the hope that it will be useful,
  17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  19  * GNU General Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License along
  22  * with MPlayer; if not, write to the Free Software Foundation, Inc.,
  23  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  24  */
  25
  26 #include <stdio.h>
  27 #include <stdlib.h>
  28
  29 #include <inttypes.h>
  30 #include <math.h>
  31
  32 #include "af.h"
  33
  34 #define L       2      // Storage for filter taps
  35 #define KM      10     // Max number of bands
  36
  37 #define Q   1.2247449 /* Q value for band-pass filters 1.2247=(3/2)^(1/2)
  38                          gives 4dB suppression @ Fc*2 and Fc/2 */
  39
  40 /* Center frequencies for band-pass filters
  41    The different frequency bands are:
  42    nr.          center frequency
  43    0    31.25 Hz
  44    1    62.50 Hz
  45    2    125.0 Hz
  46    3    250.0 Hz
  47    4    500.0 Hz
  48    5    1.000 kHz
  49    6    2.000 kHz
  50    7    4.000 kHz
  51    8    8.000 kHz
  52    9    16.00 kHz
  53 */
  54 #define CF      {31.25,62.5,125,250,500,1000,2000,4000,8000,16000}
  55
  56 // Maximum and minimum gain for the bands
  57 #define G_MAX   +12.0
  58 #define G_MIN   -12.0
  59
  60 // Data for specific instances of this filter
  61 typedef struct af_equalizer_s
  62 {
  63   float   a[KM][L];             // A weights
  64   float   b[KM][L];             // B weights
  65   float   wq[AF_NCH][KM][L];    // Circular buffer for W data
  66   float   g[AF_NCH][KM];        // Gain factor for each channel and band
  67   int     K;                    // Number of used eq bands
  68   int     channels;             // Number of channels
  69   float   gain_factor;     // applied at output to avoid clipping
  70 } af_equalizer_t;
  71
  72 // 2nd order Band-pass Filter design
  73 static void bp2(float* a, float* b, float fc, float q){
  74   double th= 2.0 * M_PI * fc;
  75   double C = (1.0 - tan(th*q/2.0))/(1.0 + tan(th*q/2.0));
  76
  77   a[0] = (1.0 + C) * cos(th);
  78   a[1] = -1 * C;
  79
  80   b[0] = (1.0 - C)/2.0;
  81   b[1] = -1.0050;
  82 }
  83
  84 // Initialization and runtime control
  85 static int control(struct af_instance_s* af, int cmd, void* arg)
  86 {
  87   af_equalizer_t* s   = (af_equalizer_t*)af->setup;
  88
  89   switch(cmd){
  90   case AF_CONTROL_REINIT:{
  91     int k =0, i =0;
  92     float F[KM] = CF;
  93
  94     s->gain_factor=0.0;
  95
  96     // Sanity check
  97     if(!arg) return AF_ERROR;
  98
  99     af->data->rate   = ((af_data_t*)arg)->rate;
 100     af->data->nch    = ((af_data_t*)arg)->nch;
 101     af->data->format = AF_FORMAT_FLOAT_NE;
 102     af->data->bps    = 4;
 103
 104     // Calculate number of active filters
 105     s->K=KM;
 106     while(F[s->K-1] > (float)af->data->rate/2.2)
 107       s->K--;
 108
 109     if(s->K != KM)
 110       mp_msg(MSGT_AFILTER, MSGL_INFO, "[equalizer] Limiting the number of filters to"
 111              " %i due to low sample rate.\n",s->K);
 112
 113     // Generate filter taps
 114     for(k=0;k<s->K;k++)
 115       bp2(s->a[k],s->b[k],F[k]/((float)af->data->rate),Q);
 116
 117     // Calculate how much this plugin adds to the overall time delay
 118     af->delay = 2 * af->data->nch * af->data->bps;
 119
 120     // Calculate gain factor to prevent clipping at output
 121     for(k=0;k<AF_NCH;k++)
 122     {
 123         for(i=0;i<KM;i++)
 124         {
 125             if(s->gain_factor < s->g[k][i]) s->gain_factor=s->g[k][i];
 126         }
 127     }
 128
 129     s->gain_factor=log10(s->gain_factor + 1.0) * 20.0;
 130
 131     if(s->gain_factor > 0.0)
 132     {
 133         s->gain_factor=0.1+(s->gain_factor/12.0);
 134     }else{
 135         s->gain_factor=1;
 136     }
 137
 138     return af_test_output(af,arg);
 139   }
 140   case AF_CONTROL_COMMAND_LINE:{
 141     float g[10]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
 142     int i,j;
 143     sscanf((char*)arg,"%f:%f:%f:%f:%f:%f:%f:%f:%f:%f", &g[0], &g[1],
 144            &g[2], &g[3], &g[4], &g[5], &g[6], &g[7], &g[8] ,&g[9]);
 145     for(i=0;i<AF_NCH;i++){
 146       for(j=0;j<KM;j++){
 147         ((af_equalizer_t*)af->setup)->g[i][j] =
 148           pow(10.0,clamp(g[j],G_MIN,G_MAX)/20.0)-1.0;
 149       }
 150     }
 151     return AF_OK;
 152   }
 153   case AF_CONTROL_EQUALIZER_GAIN | AF_CONTROL_SET:{
 154     float* gain = ((af_control_ext_t*)arg)->arg;
 155     int    ch   = ((af_control_ext_t*)arg)->ch;
 156     int    k;
 157     if(ch >= AF_NCH || ch < 0)
 158       return AF_ERROR;
 159
 160     for(k = 0 ; k<KM ; k++)
 161       s->g[ch][k] = pow(10.0,clamp(gain[k],G_MIN,G_MAX)/20.0)-1.0;
 162
 163     return AF_OK;
 164   }
 165   case AF_CONTROL_EQUALIZER_GAIN | AF_CONTROL_GET:{
 166     float* gain = ((af_control_ext_t*)arg)->arg;
 167     int    ch   = ((af_control_ext_t*)arg)->ch;
 168     int    k;
 169     if(ch >= AF_NCH || ch < 0)
 170       return AF_ERROR;
 171
 172     for(k = 0 ; k<KM ; k++)
 173       gain[k] = log10(s->g[ch][k]+1.0) * 20.0;
 174
 175     return AF_OK;
 176   }
 177   }
 178   return AF_UNKNOWN;
 179 }
 180
 181 // Deallocate memory
 182 static void uninit(struct af_instance_s* af)
 183 {
 184     free(af->data);
 185     free(af->setup);
 186 }
 187
 188 // Filter data through filter
 189 static af_data_t* play(struct af_instance_s* af, af_data_t* data)
 190 {
 191   af_data_t*       c    = data;                         // Current working data
 192   af_equalizer_t*  s    = (af_equalizer_t*)af->setup;   // Setup
 193   uint32_t         ci   = af->data->nch;                // Index for channels
 194   uint32_t         nch  = af->data->nch;                // Number of channels
 195
 196   while(ci--){
 197     float*      g   = s->g[ci];      // Gain factor
 198     float*      in  = ((float*)c->audio)+ci;
 199     float*      out = ((float*)c->audio)+ci;
 200     float*      end = in + c->len/4; // Block loop end
 201
 202     while(in < end){
 203       register int      k  = 0;         // Frequency band index
 204       register float    yt = *in;       // Current input sample
 205       in+=nch;
 206
 207       // Run the filters
 208       for(;k<s->K;k++){
 209         // Pointer to circular buffer wq
 210         register float* wq = s->wq[ci][k];
 211         // Calculate output from AR part of current filter
 212         register float w=yt*s->b[k][0] + wq[0]*s->a[k][0] + wq[1]*s->a[k][1];
 213         // Calculate output form MA part of current filter
 214         yt+=(w + wq[1]*s->b[k][1])*g[k];
 215         // Update circular buffer
 216         wq[1] = wq[0];
 217         wq[0] = w;
 218       }
 219       // Calculate output
 220       *out=yt*s->gain_factor;
 221       out+=nch;
 222     }
 223   }
 224   return c;
 225 }
 226
 227 // Allocate memory and set function pointers
 228 static int af_open(af_instance_t* af){
 229   af->control=control;
 230   af->uninit=uninit;
 231   af->play=play;
 232   af->mul=1;
 233   af->data=calloc(1,sizeof(af_data_t));
 234   af->setup=calloc(1,sizeof(af_equalizer_t));
 235   if(af->data == NULL || af->setup == NULL)
 236     return AF_ERROR;
 237   return AF_OK;
 238 }
 239
 240 // Description of this filter
 241 af_info_t af_info_equalizer = {
 242   "Equalizer audio filter",
 243   "equalizer",
 244   "Anders",
 245   "",
 246   AF_FLAGS_NOT_REENTRANT,
 247   af_open
 248 };