libao2/pl_eq.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 plugin, 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 #define PLUGIN
  17
  18 #include <stdio.h>
  19 #include <stdlib.h>
  20 #include <unistd.h>
  21 #include <inttypes.h>
  22 #include <math.h>
  23
  24 #include "audio_out.h"
  25 #include "audio_plugin.h"
  26 #include "audio_plugin_internal.h"
  27 #include "afmt.h"
  28 #include "eq.h"
  29
  30 static ao_info_t info =
  31 {
  32         "Equalizer audio plugin",
  33         "eq",
  34         "Anders",
  35         ""
  36 };
  37
  38 LIBAO_PLUGIN_EXTERN(eq)
  39
  40
  41 #define CH  6     // Max number of channels
  42 #define L   2     // Storage for filter taps
  43 #define KM  10    // Max number of octaves
  44
  45 #define Q   1.2247 /* Q value for band-pass filters 1.2247=(3/2)^(1/2)
  46                       gives 4dB suppression @ Fc*2 and Fc/2 */
  47
  48 // Center frequencies for band-pass filters
  49 #define CF  {31.25,62.5,125,250,500,1000,2000,4000,8000,16000}
  50
  51 // local data
  52 typedef struct pl_eq_s
  53 {
  54   int16_t   a[KM][L];        // A weights
  55   int16_t   b[KM][L];        // B weights
  56   int16_t   wq[CH][KM][L];   // Circular buffer for W data
  57   int16_t   g[CH][KM];       // Gain factor for each channel and band
  58   int16_t   K;               // Number of used eq bands
  59   int       channels;        // Number of channels
  60 } pl_eq_t;
  61
  62 static pl_eq_t pl_eq;
  63
  64 // to set/get/query special features/parameters
  65 static int control(int cmd,void *arg){
  66   switch(cmd){
  67   case AOCONTROL_PLUGIN_SET_LEN:
  68     return CONTROL_OK;
  69   case AOCONTROL_PLUGIN_EQ_SET_GAIN:{
  70     float gain = ((equalizer_t*)arg)->gain;
  71     int ch     =((equalizer_t*)arg)->channel;
  72     int band   =((equalizer_t*)arg)->band;
  73     if(ch > CH || ch < 0 || band > KM || band < 0)
  74       return CONTROL_ERROR;
  75
  76     pl_eq.g[ch][band]=(int16_t) 4096 * (pow(10.0,gain/20.0)-1.0);
  77     return CONTROL_OK;
  78   }
  79   case AOCONTROL_PLUGIN_EQ_GET_GAIN:{
  80     int ch     =((equalizer_t*)arg)->channel;
  81     int band   =((equalizer_t*)arg)->band;
  82     if(ch > CH || ch < 0 || band > KM || band < 0)
  83       return CONTROL_ERROR;
  84
  85     ((equalizer_t*)arg)->gain = log10((float)pl_eq.g[ch][band]/4096.0+1) * 20.0;
  86     return CONTROL_OK;
  87   }
  88   }
  89   return CONTROL_UNKNOWN;
  90 }
  91
  92 // return rounded 16bit int
  93 static inline int16_t lround16(double n){
  94   return (int16_t)((n)>=0.0?(n)+0.5:(n)-0.5);
  95 }
  96
  97 // 2nd order Band-pass Filter design
  98 void bp2(int16_t* a, int16_t* b, float fc, float q){
  99   double th=2*3.141592654*fc;
 100   double C=(1 - tan(th*q/2))/(1 + tan(th*q/2));
 101
 102   a[0] = lround16( 16383.0 * (1 + C) * cos(th));
 103   a[1] = lround16(-16383.0 * C);
 104
 105   b[0] = lround16(-16383.0 * (C - 1)/2);
 106   b[1] = lround16(-16383.0 * 1.0050);
 107 }
 108
 109 // empty buffers
 110 static void reset(){
 111   int k,l,c;
 112   for(c=0;c<pl_eq.channels;c++)
 113     for(k=0;k<pl_eq.K;k++)
 114       for(l=0;l<L*2;l++)
 115         pl_eq.wq[c][k][l]=0;
 116 }
 117
 118 // open & setup audio device
 119 // return: 1=success 0=fail
 120 static int init(){
 121   int   k   = 0;
 122   float F[KM] = CF;
 123
 124   // Check input format
 125   if(ao_plugin_data.format != AFMT_S16_NE){
 126     fprintf(stderr,"[pl_eq] Input audio format not yet supported. \n");
 127     return 0;
 128   }
 129
 130   // Check number of channels
 131   if(ao_plugin_data.channels>CH){
 132      fprintf(stderr,"[pl_eq] Too many channels, max is 6.\n");
 133     return 0;
 134   }
 135   pl_eq.channels=ao_plugin_data.channels;
 136
 137   // Calculate number of active filters
 138   pl_eq.K=KM;
 139   while(F[pl_eq.K-1] > (float)ao_plugin_data.rate/2)
 140     pl_eq.K--;
 141
 142   // Generate filter taps
 143   for(k=0;k<pl_eq.K;k++)
 144     bp2(pl_eq.a[k],pl_eq.b[k],F[k]/((float)ao_plugin_data.rate),Q);
 145
 146   // Reset buffers
 147   reset();
 148
 149   // Tell ao_plugin how much this plugin adds to the overall time delay
 150   ao_plugin_data.delay_fix-=2/((float)pl_eq.channels*(float)ao_plugin_data.rate);
 151   // Print some cool remark of what the plugin does
 152   printf("[pl_eq] Equalizer in use.\n");
 153   return 1;
 154 }
 155
 156 // close plugin
 157 static void uninit(){
 158 }
 159
 160 // processes 'ao_plugin_data.len' bytes of 'data'
 161 // called for every block of data
 162 static int play(){
 163   uint16_t      ci      = pl_eq.channels;       // Index for channels
 164   uint16_t      nch     = pl_eq.channels;       // Number of channels
 165
 166   while(ci--){
 167     int16_t*    g       = pl_eq.g[ci];  // Gain factor
 168     int16_t*    in      = ((int16_t*)ao_plugin_data.data)+ci;
 169     int16_t*    out     = ((int16_t*)ao_plugin_data.data)+ci;
 170     int16_t*    end     = in+ao_plugin_data.len/2; // Block loop end
 171
 172     while(in < end){
 173       register int16_t k   = 0;         // Frequency band index
 174       register int32_t yt  = 0;         // Total output from filters
 175       register int16_t x   = *in;       // Current input sample
 176       in+=nch;
 177
 178       // Run the filters
 179       for(;k<pl_eq.K;k++){
 180         // Pointer to circular buffer wq
 181         register int16_t* wq = pl_eq.wq[ci][k];
 182 #if 0
 183         // Calculate output from AR part of current filter
 184         register int32_t xt = (x*pl_eq.b[k][0]) >> 4;
 185         register int32_t w = xt + wq[0]*pl_eq.a[k][0] + wq[1]*pl_eq.a[k][1];
 186         // Calculate output form MA part of current filter
 187         yt+=(((w + wq[1]*pl_eq.b[k][1]) >> 10)*g[k]) >> 12;
 188         // Update circular buffer
 189         wq[1] = wq[0]; wq[0] = w >> 14;
 190       }
 191
 192       // Calculate output
 193       *out=(int16_t)(yt+x);
 194 #else
 195         // Calculate output from AR part of current filter
 196         register int32_t xt = (x*pl_eq.b[k][0]) / 48;
 197         register int32_t w = xt + wq[0]*pl_eq.a[k][0] + wq[1]*pl_eq.a[k][1];
 198         // Calculate output form MA part of current filter
 199         yt+=(((w + wq[1]*pl_eq.b[k][1]) >> 10)*g[k]) >> 12;
 200         // Update circular buffer
 201         wq[1] = wq[0]; wq[0] = w / 24576;
 202       }
 203
 204       // Calculate output
 205       *out=(int16_t)(yt * 0.25 + x * 0.5);
 206 #endif
 207       out+=nch;
 208     }
 209   }
 210   return 1;
 211 }
 212
 213
 214
 215
 216
 217
 218