libaf/af_equalizer.c

   1 /*=============================================================================
   2 //
   3 //  This software has been released under the terms of the GNU 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 } af_equalizer_t;
  62
  63 // 2nd order Band-pass Filter design
  64 static void bp2(float* a, float* b, float fc, float q){
  65   double th= 2.0 * M_PI * fc;
  66   double C = (1.0 - tan(th*q/2.0))/(1.0 + tan(th*q/2.0));
  67
  68   a[0] = (1.0 + C) * cos(th);
  69   a[1] = -1 * C;
  70
  71   b[0] = (1.0 - C)/2.0;
  72   b[1] = -1.0050;
  73 }
  74
  75 // Initialization and runtime control
  76 static int control(struct af_instance_s* af, int cmd, void* arg)
  77 {
  78   af_equalizer_t* s   = (af_equalizer_t*)af->setup;
  79
  80   switch(cmd){
  81   case AF_CONTROL_REINIT:{
  82     int k =0;
  83     float F[KM] = CF;
  84
  85     // Sanity check
  86     if(!arg) return AF_ERROR;
  87
  88     af->data->rate   = ((af_data_t*)arg)->rate;
  89     af->data->nch    = ((af_data_t*)arg)->nch;
  90     af->data->format = AF_FORMAT_NE | AF_FORMAT_F;
  91     af->data->bps    = 4;
  92
  93     // Calculate number of active filters
  94     s->K=KM;
  95     while(F[s->K-1] > (float)af->data->rate/2.2)
  96       s->K--;
  97
  98     if(s->K != KM)
  99       af_msg(AF_MSG_INFO,"[equalizer] Limiting the number of filters to"
 100              " %i due to low sample rate.\n",s->K);
 101
 102     // Generate filter taps
 103     for(k=0;k<s->K;k++)
 104       bp2(s->a[k],s->b[k],F[k]/((float)af->data->rate),Q);
 105
 106     // Calculate how much this plugin adds to the overall time delay
 107     af->delay += 2000.0/((float)af->data->rate);
 108
 109     return af_test_output(af,arg);
 110   }
 111   case AF_CONTROL_COMMAND_LINE:{
 112     float g[10]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
 113     int i,j;
 114     sscanf((char*)arg,"%f:%f:%f:%f:%f:%f:%f:%f:%f:%f", &g[0], &g[1],
 115            &g[2], &g[3], &g[4], &g[5], &g[6], &g[7], &g[8] ,&g[9]);
 116     for(i=0;i<AF_NCH;i++){
 117       for(j=0;j<KM;j++){
 118         ((af_equalizer_t*)af->setup)->g[i][j] =
 119           pow(10.0,clamp(g[j],G_MIN,G_MAX)/20.0)-1.0;
 120       }
 121     }
 122     return AF_OK;
 123   }
 124   case AF_CONTROL_EQUALIZER_GAIN | AF_CONTROL_SET:{
 125     float* gain = ((af_control_ext_t*)arg)->arg;
 126     int    ch   = ((af_control_ext_t*)arg)->ch;
 127     int    k;
 128     if(ch > AF_NCH || ch < 0)
 129       return AF_ERROR;
 130
 131     for(k = 0 ; k<KM ; k++)
 132       s->g[ch][k] = pow(10.0,clamp(gain[k],G_MIN,G_MAX)/20.0)-1.0;
 133
 134     return AF_OK;
 135   }
 136   case AF_CONTROL_EQUALIZER_GAIN | AF_CONTROL_GET:{
 137     float* gain = ((af_control_ext_t*)arg)->arg;
 138     int    ch   = ((af_control_ext_t*)arg)->ch;
 139     int    k;
 140     if(ch > AF_NCH || ch < 0)
 141       return AF_ERROR;
 142
 143     for(k = 0 ; k<KM ; k++)
 144       gain[k] = log10(s->g[ch][k]+1.0) * 20.0;
 145
 146     return AF_OK;
 147   }
 148   }
 149   return AF_UNKNOWN;
 150 }
 151
 152 // Deallocate memory
 153 static void uninit(struct af_instance_s* af)
 154 {
 155   if(af->data)
 156     free(af->data);
 157   if(af->setup)
 158     free(af->setup);
 159 }
 160
 161 // Filter data through filter
 162 static af_data_t* play(struct af_instance_s* af, af_data_t* data)
 163 {
 164   af_data_t*       c    = data;                         // Current working data
 165   af_equalizer_t*  s    = (af_equalizer_t*)af->setup;   // Setup
 166   uint32_t         ci   = af->data->nch;                // Index for channels
 167   uint32_t         nch  = af->data->nch;                // Number of channels
 168
 169   while(ci--){
 170     float*      g   = s->g[ci];      // Gain factor
 171     float*      in  = ((float*)c->audio)+ci;
 172     float*      out = ((float*)c->audio)+ci;
 173     float*      end = in + c->len/4; // Block loop end
 174
 175     while(in < end){
 176       register uint32_t k  = 0;         // Frequency band index
 177       register float    yt = *in;       // Current input sample
 178       in+=nch;
 179
 180       // Run the filters
 181       for(;k<s->K;k++){
 182         // Pointer to circular buffer wq
 183         register float* wq = s->wq[ci][k];
 184         // Calculate output from AR part of current filter
 185         register float w=yt*s->b[k][0] + wq[0]*s->a[k][0] + wq[1]*s->a[k][1];
 186         // Calculate output form MA part of current filter
 187         yt+=(w + wq[1]*s->b[k][1])*g[k];
 188         // Update circular buffer
 189         wq[1] = wq[0];
 190         wq[0] = w;
 191       }
 192       // Calculate output
 193       *out=yt/(4.0*10.0);
 194       out+=nch;
 195     }
 196   }
 197   return c;
 198 }
 199
 200 // Allocate memory and set function pointers
 201 static int open(af_instance_t* af){
 202   af->control=control;
 203   af->uninit=uninit;
 204   af->play=play;
 205   af->mul.n=1;
 206   af->mul.d=1;
 207   af->data=calloc(1,sizeof(af_data_t));
 208   af->setup=calloc(1,sizeof(af_equalizer_t));
 209   if(af->data == NULL || af->setup == NULL)
 210     return AF_ERROR;
 211   return AF_OK;
 212 }
 213
 214 // Description of this filter
 215 af_info_t af_info_equalizer = {
 216   "Equalizer audio filter",
 217   "equalizer",
 218   "Anders",
 219   "",
 220   AF_FLAGS_NOT_REENTRANT,
 221   open
 222 };
 223
 224
 225
 226
 227
 228
 229