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
  29 #ifdef HAVE_CONFIG_H
  30 # include "config.h"
  31 #endif
  32
  33 #include <math.h>
  34
  35 #include <vlc_common.h>
  36 #include <vlc_plugin.h>
  37 #include <vlc_aout.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 void DoWork( aout_instance_t *, aout_filter_t *,
  49                     aout_buffer_t *, aout_buffer_t * );
  50
  51 vlc_module_begin ()
  52     set_description( N_("Parametric Equalizer") )
  53     set_shortname( N_("Parametric Equalizer" ) )
  54     set_capability( "audio filter", 0 )
  55     set_category( CAT_AUDIO )
  56     set_subcategory( SUBCAT_AUDIO_AFILTER )
  57
  58     add_float( "param-eq-lowf", 100, NULL, N_("Low freq (Hz)"),"", false )
  59     add_float_with_range( "param-eq-lowgain", 0, -20.0, 20.0, NULL,
  60                           N_("Low freq gain (dB)"), "",false )
  61     add_float( "param-eq-highf", 10000, NULL, N_("High freq (Hz)"),"", false )
  62     add_float_with_range( "param-eq-highgain", 0, -20.0, 20.0, NULL,
  63                           N_("High freq gain (dB)"),"",false )
  64     add_float( "param-eq-f1", 300, NULL, N_("Freq 1 (Hz)"),"", false )
  65     add_float_with_range( "param-eq-gain1", 0, -20.0, 20.0, NULL,
  66                           N_("Freq 1 gain (dB)"), "",false )
  67     add_float_with_range( "param-eq-q1", 3, 0.1, 100.0, NULL,
  68                           N_("Freq 1 Q"), "",false )
  69     add_float( "param-eq-f2", 1000, NULL, N_("Freq 2 (Hz)"),"", false )
  70     add_float_with_range( "param-eq-gain2", 0, -20.0, 20.0, NULL,
  71                           N_("Freq 2 gain (dB)"),"",false )
  72     add_float_with_range( "param-eq-q2", 3, 0.1, 100.0, NULL,
  73                           N_("Freq 2 Q"),"",false )
  74     add_float( "param-eq-f3", 3000, NULL, N_("Freq 3 (Hz)"),"", false )
  75     add_float_with_range( "param-eq-gain3", 0, -20.0, 20.0, NULL,
  76                           N_("Freq 3 gain (dB)"),"",false )
  77     add_float_with_range( "param-eq-q3", 3, 0.1, 100.0, NULL,
  78                           N_("Freq 3 Q"),"",false )
  79
  80     set_callbacks( Open, Close )
  81 vlc_module_end ()
  82
  83 /*****************************************************************************
  84  * Local prototypes
  85  *****************************************************************************/
  86 struct aout_filter_sys_t
  87 {
  88     /* Filter static config */
  89     float   f_lowf, f_lowgain;
  90     float   f_f1, f_Q1, f_gain1;
  91     float   f_f2, f_Q2, f_gain2;
  92     float   f_f3, f_Q3, f_gain3;
  93     float   f_highf, f_highgain;
  94     /* Filter computed coeffs */
  95     float   coeffs[5*5];
  96     /* State */
  97     float  *p_state;
  98
  99 };
 100
 101
 102
 103
 104 /*****************************************************************************
 105  * Open:
 106  *****************************************************************************/
 107 static int Open( vlc_object_t *p_this )
 108 {
 109     aout_filter_t     *p_filter = (aout_filter_t *)p_this;
 110     aout_filter_sys_t *p_sys;
 111     bool         b_fit = true;
 112     int                i_samplerate;
 113
 114     if( p_filter->fmt_in.audio.i_format != VLC_CODEC_FL32 ||
 115         p_filter->fmt_out.audio.i_format != VLC_CODEC_FL32 )
 116     {
 117         b_fit = false;
 118         p_filter->fmt_in.audio.i_format = VLC_CODEC_FL32;
 119         p_filter->fmt_out.audio.i_format = VLC_CODEC_FL32;
 120         msg_Warn( p_filter, "bad input or output format" );
 121     }
 122     if ( !AOUT_FMTS_SIMILAR( &p_filter->fmt_in.audio, &p_filter->fmt_out.audio ) )
 123     {
 124         b_fit = false;
 125         memcpy( &p_filter->fmt_out.audio, &p_filter->fmt_in.audio,
 126                 sizeof(audio_sample_format_t) );
 127         msg_Warn( p_filter, "input and output formats are not similar" );
 128     }
 129
 130     if ( ! b_fit )
 131     {
 132         return VLC_EGENERIC;
 133     }
 134
 135     p_filter->pf_do_work = DoWork;
 136     p_filter->b_in_place = true;
 137
 138     /* Allocate structure */
 139     p_sys = p_filter->p_sys = malloc( sizeof( aout_filter_sys_t ) );
 140
 141     p_sys->f_lowf = config_GetFloat( p_this, "param-eq-lowf");
 142     p_sys->f_lowgain = config_GetFloat( p_this, "param-eq-lowgain");
 143     p_sys->f_highf = config_GetFloat( p_this, "param-eq-highf");
 144     p_sys->f_highgain = config_GetFloat( p_this, "param-eq-highgain");
 145
 146     p_sys->f_f1 = config_GetFloat( p_this, "param-eq-f1");
 147     p_sys->f_Q1 = config_GetFloat( p_this, "param-eq-q1");
 148     p_sys->f_gain1 = config_GetFloat( p_this, "param-eq-gain1");
 149
 150     p_sys->f_f2 = config_GetFloat( p_this, "param-eq-f2");
 151     p_sys->f_Q2 = config_GetFloat( p_this, "param-eq-q2");
 152     p_sys->f_gain2 = config_GetFloat( p_this, "param-eq-gain2");
 153
 154     p_sys->f_f3 = config_GetFloat( p_this, "param-eq-f3");
 155     p_sys->f_Q3 = config_GetFloat( p_this, "param-eq-q3");
 156     p_sys->f_gain3 = config_GetFloat( p_this, "param-eq-gain3");
 157
 158
 159     i_samplerate = p_filter->fmt_in.audio.i_rate;
 160     CalcPeakEQCoeffs(p_sys->f_f1, p_sys->f_Q1, p_sys->f_gain1,
 161                      i_samplerate, p_sys->coeffs+0*5);
 162     CalcPeakEQCoeffs(p_sys->f_f2, p_sys->f_Q2, p_sys->f_gain2,
 163                      i_samplerate, p_sys->coeffs+1*5);
 164     CalcPeakEQCoeffs(p_sys->f_f3, p_sys->f_Q3, p_sys->f_gain3,
 165                      i_samplerate, p_sys->coeffs+2*5);
 166     CalcShelfEQCoeffs(p_sys->f_lowf, 1, p_sys->f_lowgain, 0,
 167                       i_samplerate, p_sys->coeffs+3*5);
 168     CalcShelfEQCoeffs(p_sys->f_highf, 1, p_sys->f_highgain, 0,
 169                       i_samplerate, p_sys->coeffs+4*5);
 170     p_sys->p_state = (float*)calloc( p_filter->fmt_in.audio.i_channels*5*4,
 171                                      sizeof(float) );
 172
 173     return VLC_SUCCESS;
 174 }
 175
 176 static void Close( vlc_object_t *p_this )
 177 {
 178     aout_filter_t *p_filter = (aout_filter_t *)p_this;
 179     free( p_filter->p_sys->p_state );
 180     free( p_filter->p_sys );
 181 }
 182
 183 /*****************************************************************************
 184  * DoWork: process samples buffer
 185  *****************************************************************************
 186  *
 187  *****************************************************************************/
 188 static void DoWork( aout_instance_t * p_aout, aout_filter_t * p_filter,
 189                     aout_buffer_t * p_in_buf, aout_buffer_t * p_out_buf )
 190 {
 191     VLC_UNUSED(p_aout);
 192     p_out_buf->i_nb_samples = p_in_buf->i_nb_samples;
 193     p_out_buf->i_buffer = p_in_buf->i_buffer;
 194
 195     ProcessEQ( (float*)p_in_buf->p_buffer, (float*)p_out_buf->p_buffer,
 196                p_filter->p_sys->p_state,
 197                p_filter->fmt_in.audio.i_channels, p_in_buf->i_nb_samples,
 198                p_filter->p_sys->coeffs, 5 );
 199 }
 200
 201 /*
 202  * Calculate direct form IIR coefficients for peaking EQ
 203  * coeffs[0] = b0
 204  * coeffs[1] = b1
 205  * coeffs[2] = b2
 206  * coeffs[3] = a1
 207  * coeffs[4] = a2
 208  *
 209  * Equations taken from RBJ audio EQ cookbook
 210  * (http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt)
 211  */
 212 static void CalcPeakEQCoeffs( float f0, float Q, float gainDB, float Fs,
 213                               float *coeffs )
 214 {
 215     float A;
 216     float w0;
 217     float alpha;
 218     float b0, b1, b2;
 219     float a0, a1, a2;
 220
 221     // Provide sane limits to avoid overflow
 222     if (Q < 0.1f) Q = 0.1f;
 223     if (Q > 100) Q = 100;
 224     if (f0 > Fs/2*0.95f) f0 = Fs/2*0.95f;
 225     if (gainDB < -40) gainDB = -40;
 226     if (gainDB > 40) gainDB = 40;
 227
 228     A = pow(10, gainDB/40);
 229     w0 = 2*3.141593f*f0/Fs;
 230     alpha = sin(w0)/(2*Q);
 231
 232     b0 = 1 + alpha*A;
 233     b1 = -2*cos(w0);
 234     b2 = 1 - alpha*A;
 235     a0 = 1 + alpha/A;
 236     a1 = -2*cos(w0);
 237     a2 = 1 - alpha/A;
 238
 239     // Store values to coeffs and normalize by 1/a0
 240     coeffs[0] = b0/a0;
 241     coeffs[1] = b1/a0;
 242     coeffs[2] = b2/a0;
 243     coeffs[3] = a1/a0;
 244     coeffs[4] = a2/a0;
 245 }
 246
 247 /*
 248  * Calculate direct form IIR coefficients for low/high shelf EQ
 249  * coeffs[0] = b0
 250  * coeffs[1] = b1
 251  * coeffs[2] = b2
 252  * coeffs[3] = a1
 253  * coeffs[4] = a2
 254  *
 255  * Equations taken from RBJ audio EQ cookbook
 256  * (http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt)
 257  */
 258 static void CalcShelfEQCoeffs( float f0, float slope, float gainDB, int high,
 259                                float Fs, float *coeffs )
 260 {
 261     float A;
 262     float w0;
 263     float alpha;
 264     float b0, b1, b2;
 265     float a0, a1, a2;
 266
 267     // Provide sane limits to avoid overflow
 268     if (f0 > Fs/2*0.95f) f0 = Fs/2*0.95f;
 269     if (gainDB < -40) gainDB = -40;
 270     if (gainDB > 40) gainDB = 40;
 271
 272     A = pow(10, gainDB/40);
 273     w0 = 2*3.141593f*f0/Fs;
 274     alpha = sin(w0)/2 * sqrt( (A + 1/A)*(1/slope - 1) + 2 );
 275
 276     if (high)
 277     {
 278         b0 =    A*( (A+1) + (A-1)*cos(w0) + 2*sqrt(A)*alpha );
 279         b1 = -2*A*( (A-1) + (A+1)*cos(w0) );
 280         b2 =    A*( (A+1) + (A-1)*cos(w0) - 2*sqrt(A)*alpha );
 281         a0 =        (A+1) - (A-1)*cos(w0) + 2*sqrt(A)*alpha;
 282         a1 =    2*( (A-1) - (A+1)*cos(w0) );
 283         a2 =        (A+1) - (A-1)*cos(w0) - 2*sqrt(A)*alpha;
 284     }
 285     else
 286     {
 287         b0 =    A*( (A+1) - (A-1)*cos(w0) + 2*sqrt(A)*alpha );
 288         b1 =  2*A*( (A-1) - (A+1)*cos(w0));
 289         b2 =    A*( (A+1) - (A-1)*cos(w0) - 2*sqrt(A)*alpha );
 290         a0 =        (A+1) + (A-1)*cos(w0) + 2*sqrt(A)*alpha;
 291         a1 =   -2*( (A-1) + (A+1)*cos(w0));
 292         a2 =        (A+1) + (A-1)*cos(w0) - 2*sqrt(A)*alpha;
 293     }
 294     // Store values to coeffs and normalize by 1/a0
 295     coeffs[0] = b0/a0;
 296     coeffs[1] = b1/a0;
 297     coeffs[2] = b2/a0;
 298     coeffs[3] = a1/a0;
 299     coeffs[4] = a2/a0;
 300 }
 301
 302 /*
 303   src is assumed to be interleaved
 304   dest is assumed to be interleaved
 305   size of state is 4*channels*eqCount
 306   samples is not premultiplied by channels
 307   size of coeffs is 5*eqCount
 308 */
 309 void ProcessEQ( const float *src, float *dest, float *state,
 310                 unsigned channels, unsigned samples, const float *coeffs,
 311                 unsigned eqCount )
 312 {
 313     unsigned i, chn, eq;
 314     float   b0, b1, b2, a1, a2;
 315     float   x, y = 0;
 316     const float *src1 = src;
 317     float *dest1 = dest;
 318
 319     for (i = 0; i < samples; i++)
 320     {
 321         float *state1 = state;
 322         for (chn = 0; chn < channels; chn++)
 323         {
 324             const float *coeffs1 = coeffs;
 325             x = *src1++;
 326             /* Direct form 1 IIRs */
 327             for (eq = 0; eq < eqCount; eq++)
 328             {
 329                 b0 = coeffs1[0];
 330                 b1 = coeffs1[1];
 331                 b2 = coeffs1[2];
 332                 a1 = coeffs1[3];
 333                 a2 = coeffs1[4];
 334                 coeffs1 += 5;
 335                 y = x*b0 + state1[0]*b1 + state1[1]*b2 - state1[2]*a1 - state1[3]*a2;
 336                 state1[1] = state1[0];
 337                 state1[0] = x;
 338                 state1[3] = state1[2];
 339                 state1[2] = y;
 340                 x = y;
 341                 state1 += 4;
 342             }
 343             *dest1++ = y;
 344         }
 345     }
 346 }
 347