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