git.sesse.net Git - vlc/blob - modules/audio_filter/param_eq.c

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