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