libaf/af_equalizer.c

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