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