modules/audio_filter/param_eq.c

   1 /*****************************************************************************
   2  * param_eq.c:
   3  *****************************************************************************
   4  * Copyright © 2006 VLC authors and VideoLAN
   5  *
   6  * Authors: Antti Huovilainen
   7  *          Sigmund A. Helberg <dnumgis@videolan.org>
   8  *
   9  * This program is free software; you can redistribute it and/or modify it
  10  * under the terms of the GNU Lesser General Public License as published by
  11  * the Free Software Foundation; either version 2.1 of the License, or
  12  * (at your option) any later version.
  13  *
  14  * This program is distributed in the hope that it will be useful,
  15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  17  * GNU Lesser General Public License for more details.
  18  *
  19  * You should have received a copy of the GNU Lesser General Public License
  20  * along with this program; if not, write to the Free Software Foundation,
  21  * Inc., 51 Franklin Street, Fifth Floor, Boston MA 02110-1301, USA.
  22  *****************************************************************************/
  23
  24 /*****************************************************************************
  25  * Preamble
  26  *****************************************************************************/
  27
  28 #ifdef HAVE_CONFIG_H
  29 # include "config.h"
  30 #endif
  31
  32 #include <math.h>
  33
  34 #include <vlc_common.h>
  35 #include <vlc_plugin.h>
  36 #include <vlc_aout.h>
  37 #include <vlc_filter.h>
  38
  39 /*****************************************************************************
  40  * Module descriptor
  41  *****************************************************************************/
  42 static int  Open ( vlc_object_t * );
  43 static void Close( vlc_object_t * );
  44 static void CalcPeakEQCoeffs( float, float, float, float, float * );
  45 static void CalcShelfEQCoeffs( float, float, float, int, float, float * );
  46 static void ProcessEQ( const float *, float *, float *, unsigned, unsigned,
  47                        const float *, unsigned );
  48 static block_t *DoWork( filter_t *, block_t * );
  49
  50 vlc_module_begin ()
  51     set_description( N_("Parametric Equalizer") )
  52     set_shortname( N_("Parametric Equalizer" ) )
  53     set_capability( "audio filter", 0 )
  54     set_category( CAT_AUDIO )
  55     set_subcategory( SUBCAT_AUDIO_AFILTER )
  56
  57     add_float( "param-eq-lowf", 100, N_("Low freq (Hz)"),NULL, false )
  58     add_float_with_range( "param-eq-lowgain", 0, -20.0, 20.0,
  59                           N_("Low freq gain (dB)"), NULL,false )
  60     add_float( "param-eq-highf", 10000, N_("High freq (Hz)"),NULL, false )
  61     add_float_with_range( "param-eq-highgain", 0, -20.0, 20.0,
  62                           N_("High freq gain (dB)"),NULL,false )
  63     add_float( "param-eq-f1", 300, N_("Freq 1 (Hz)"),NULL, false )
  64     add_float_with_range( "param-eq-gain1", 0, -20.0, 20.0,
  65                           N_("Freq 1 gain (dB)"), NULL,false )
  66     add_float_with_range( "param-eq-q1", 3, 0.1, 100.0,
  67                           N_("Freq 1 Q"), NULL,false )
  68     add_float( "param-eq-f2", 1000, N_("Freq 2 (Hz)"),NULL, false )
  69     add_float_with_range( "param-eq-gain2", 0, -20.0, 20.0,
  70                           N_("Freq 2 gain (dB)"),NULL,false )
  71     add_float_with_range( "param-eq-q2", 3, 0.1, 100.0,
  72                           N_("Freq 2 Q"),NULL,false )
  73     add_float( "param-eq-f3", 3000, N_("Freq 3 (Hz)"),NULL, false )
  74     add_float_with_range( "param-eq-gain3", 0, -20.0, 20.0,
  75                           N_("Freq 3 gain (dB)"),NULL,false )
  76     add_float_with_range( "param-eq-q3", 3, 0.1, 100.0,
  77                           N_("Freq 3 Q"),NULL,false )
  78
  79     set_callbacks( Open, Close )
  80 vlc_module_end ()
  81
  82 /*****************************************************************************
  83  * Local prototypes
  84  *****************************************************************************/
  85 typedef struct
  86 {
  87     /* Filter static config */
  88     float   f_lowf, f_lowgain;
  89     float   f_f1, f_Q1, f_gain1;
  90     float   f_f2, f_Q2, f_gain2;
  91     float   f_f3, f_Q3, f_gain3;
  92     float   f_highf, f_highgain;
  93     /* Filter computed coeffs */
  94     float   coeffs[5*5];
  95     /* State */
  96     float  *p_state;
  97 } filter_sys_t;
  98
  99
 100
 101
 102 /*****************************************************************************
 103  * Open:
 104  *****************************************************************************/
 105 static int Open( vlc_object_t *p_this )
 106 {
 107     filter_t     *p_filter = (filter_t *)p_this;
 108     unsigned     i_samplerate;
 109
 110     /* Allocate structure */
 111     filter_sys_t *p_sys = p_filter->p_sys = malloc( sizeof( *p_sys ) );
 112     if( !p_sys )
 113         return VLC_EGENERIC;
 114
 115     p_filter->fmt_in.audio.i_format = VLC_CODEC_FL32;
 116     p_filter->fmt_out.audio = p_filter->fmt_in.audio;
 117     p_filter->pf_audio_filter = DoWork;
 118
 119     p_sys->f_lowf = var_InheritFloat( p_this, "param-eq-lowf");
 120     p_sys->f_lowgain = var_InheritFloat( p_this, "param-eq-lowgain");
 121     p_sys->f_highf = var_InheritFloat( p_this, "param-eq-highf");
 122     p_sys->f_highgain = var_InheritFloat( p_this, "param-eq-highgain");
 123
 124     p_sys->f_f1 = var_InheritFloat( p_this, "param-eq-f1");
 125     p_sys->f_Q1 = var_InheritFloat( p_this, "param-eq-q1");
 126     p_sys->f_gain1 = var_InheritFloat( p_this, "param-eq-gain1");
 127
 128     p_sys->f_f2 = var_InheritFloat( p_this, "param-eq-f2");
 129     p_sys->f_Q2 = var_InheritFloat( p_this, "param-eq-q2");
 130     p_sys->f_gain2 = var_InheritFloat( p_this, "param-eq-gain2");
 131
 132     p_sys->f_f3 = var_InheritFloat( p_this, "param-eq-f3");
 133     p_sys->f_Q3 = var_InheritFloat( p_this, "param-eq-q3");
 134     p_sys->f_gain3 = var_InheritFloat( p_this, "param-eq-gain3");
 135
 136
 137     i_samplerate = p_filter->fmt_in.audio.i_rate;
 138     CalcPeakEQCoeffs(p_sys->f_f1, p_sys->f_Q1, p_sys->f_gain1,
 139                      i_samplerate, p_sys->coeffs+0*5);
 140     CalcPeakEQCoeffs(p_sys->f_f2, p_sys->f_Q2, p_sys->f_gain2,
 141                      i_samplerate, p_sys->coeffs+1*5);
 142     CalcPeakEQCoeffs(p_sys->f_f3, p_sys->f_Q3, p_sys->f_gain3,
 143                      i_samplerate, p_sys->coeffs+2*5);
 144     CalcShelfEQCoeffs(p_sys->f_lowf, 1, p_sys->f_lowgain, 0,
 145                       i_samplerate, p_sys->coeffs+3*5);
 146     CalcShelfEQCoeffs(p_sys->f_highf, 1, p_sys->f_highgain, 0,
 147                       i_samplerate, p_sys->coeffs+4*5);
 148     p_sys->p_state = (float*)calloc( p_filter->fmt_in.audio.i_channels*5*4,
 149                                      sizeof(float) );
 150
 151     return VLC_SUCCESS;
 152 }
 153
 154 static void Close( vlc_object_t *p_this )
 155 {
 156     filter_t *p_filter = (filter_t *)p_this;
 157     filter_sys_t *p_sys = p_filter->p_sys;
 158     free( p_sys->p_state );
 159     free( p_sys );
 160 }
 161
 162 /*****************************************************************************
 163  * DoWork: process samples buffer
 164  *****************************************************************************
 165  *
 166  *****************************************************************************/
 167 static block_t *DoWork( filter_t * p_filter, block_t * p_in_buf )
 168 {
 169     filter_sys_t *p_sys = p_filter->p_sys;
 170     ProcessEQ( (float*)p_in_buf->p_buffer, (float*)p_in_buf->p_buffer,
 171                p_sys->p_state,
 172                p_filter->fmt_in.audio.i_channels, p_in_buf->i_nb_samples,
 173                p_sys->coeffs, 5 );
 174     return p_in_buf;
 175 }
 176
 177 /*
 178  * Calculate direct form IIR coefficients for peaking EQ
 179  * coeffs[0] = b0
 180  * coeffs[1] = b1
 181  * coeffs[2] = b2
 182  * coeffs[3] = a1
 183  * coeffs[4] = a2
 184  *
 185  * Equations taken from RBJ audio EQ cookbook
 186  * (http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt)
 187  */
 188 static void CalcPeakEQCoeffs( float f0, float Q, float gainDB, float Fs,
 189                               float *coeffs )
 190 {
 191     float A;
 192     float w0;
 193     float alpha;
 194     float b0, b1, b2;
 195     float a0, a1, a2;
 196
 197     // Provide sane limits to avoid overflow
 198     if (Q < 0.1f) Q = 0.1f;
 199     if (Q > 100) Q = 100;
 200     if (f0 > Fs/2*0.95f) f0 = Fs/2*0.95f;
 201     if (gainDB < -40) gainDB = -40;
 202     if (gainDB > 40) gainDB = 40;
 203
 204     A = powf(10, gainDB/40);
 205     w0 = 2*((float)M_PI)*f0/Fs;
 206     alpha = sinf(w0)/(2*Q);
 207
 208     b0 = 1 + alpha*A;
 209     b1 = -2*cosf(w0);
 210     b2 = 1 - alpha*A;
 211     a0 = 1 + alpha/A;
 212     a1 = -2*cosf(w0);
 213     a2 = 1 - alpha/A;
 214
 215     // Store values to coeffs and normalize by 1/a0
 216     coeffs[0] = b0/a0;
 217     coeffs[1] = b1/a0;
 218     coeffs[2] = b2/a0;
 219     coeffs[3] = a1/a0;
 220     coeffs[4] = a2/a0;
 221 }
 222
 223 /*
 224  * Calculate direct form IIR coefficients for low/high shelf EQ
 225  * coeffs[0] = b0
 226  * coeffs[1] = b1
 227  * coeffs[2] = b2
 228  * coeffs[3] = a1
 229  * coeffs[4] = a2
 230  *
 231  * Equations taken from RBJ audio EQ cookbook
 232  * (http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt)
 233  */
 234 static void CalcShelfEQCoeffs( float f0, float slope, float gainDB, int high,
 235                                float Fs, float *coeffs )
 236 {
 237     float A;
 238     float w0;
 239     float alpha;
 240     float b0, b1, b2;
 241     float a0, a1, a2;
 242
 243     // Provide sane limits to avoid overflow
 244     if (f0 > Fs/2*0.95f) f0 = Fs/2*0.95f;
 245     if (gainDB < -40) gainDB = -40;
 246     if (gainDB > 40) gainDB = 40;
 247
 248     A = powf(10, gainDB/40);
 249     w0 = 2*3.141593f*f0/Fs;
 250     alpha = sinf(w0)/2 * sqrtf( (A + 1/A)*(1/slope - 1) + 2 );
 251
 252     if (high)
 253     {
 254         b0 =    A*( (A+1) + (A-1)*cosf(w0) + 2*sqrtf(A)*alpha );
 255         b1 = -2*A*( (A-1) + (A+1)*cosf(w0) );
 256         b2 =    A*( (A+1) + (A-1)*cosf(w0) - 2*sqrtf(A)*alpha );
 257         a0 =        (A+1) - (A-1)*cosf(w0) + 2*sqrtf(A)*alpha;
 258         a1 =    2*( (A-1) - (A+1)*cosf(w0) );
 259         a2 =        (A+1) - (A-1)*cosf(w0) - 2*sqrtf(A)*alpha;
 260     }
 261     else
 262     {
 263         b0 =    A*( (A+1) - (A-1)*cosf(w0) + 2*sqrtf(A)*alpha );
 264         b1 =  2*A*( (A-1) - (A+1)*cosf(w0));
 265         b2 =    A*( (A+1) - (A-1)*cosf(w0) - 2*sqrtf(A)*alpha );
 266         a0 =        (A+1) + (A-1)*cosf(w0) + 2*sqrtf(A)*alpha;
 267         a1 =   -2*( (A-1) + (A+1)*cosf(w0));
 268         a2 =        (A+1) + (A-1)*cosf(w0) - 2*sqrtf(A)*alpha;
 269     }
 270     // Store values to coeffs and normalize by 1/a0
 271     coeffs[0] = b0/a0;
 272     coeffs[1] = b1/a0;
 273     coeffs[2] = b2/a0;
 274     coeffs[3] = a1/a0;
 275     coeffs[4] = a2/a0;
 276 }
 277
 278 /*
 279   src is assumed to be interleaved
 280   dest is assumed to be interleaved
 281   size of state is 4*channels*eqCount
 282   samples is not premultiplied by channels
 283   size of coeffs is 5*eqCount
 284 */
 285 void ProcessEQ( const float *src, float *dest, float *state,
 286                 unsigned channels, unsigned samples, const float *coeffs,
 287                 unsigned eqCount )
 288 {
 289     unsigned i, chn, eq;
 290     float   b0, b1, b2, a1, a2;
 291     float   x, y = 0;
 292     const float *src1 = src;
 293     float *dest1 = dest;
 294
 295     for (i = 0; i < samples; i++)
 296     {
 297         float *state1 = state;
 298         for (chn = 0; chn < channels; chn++)
 299         {
 300             const float *coeffs1 = coeffs;
 301             x = *src1++;
 302             /* Direct form 1 IIRs */
 303             for (eq = 0; eq < eqCount; eq++)
 304             {
 305                 b0 = coeffs1[0];
 306                 b1 = coeffs1[1];
 307                 b2 = coeffs1[2];
 308                 a1 = coeffs1[3];
 309                 a2 = coeffs1[4];
 310                 coeffs1 += 5;
 311                 y = x*b0 + state1[0]*b1 + state1[1]*b2 - state1[2]*a1 - state1[3]*a2;
 312                 state1[1] = state1[0];
 313                 state1[0] = x;
 314                 state1[3] = state1[2];
 315                 state1[2] = y;
 316                 x = y;
 317                 state1 += 4;
 318             }
 319             *dest1++ = y;
 320         }
 321     }
 322 }
 323