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