audio: add af_lavrresample, remove old resampling filters
[mplayer2.git] / libaf / af_equalizer.c
blob318b7a72d095223e31607812e709455bbddd1ee6
1 /*
2 * Equalizer filter, implementation of a 10 band time domain graphic
3 * equalizer using IIR filters. The IIR filters are implemented using a
4 * Direct Form II approach, but has been modified (b1 == 0 always) to
5 * save computation.
7 * Copyright (C) 2001 Anders Johansson ajh@atri.curtin.edu.au
9 * This file is part of MPlayer.
11 * MPlayer is free software; you can redistribute it and/or modify
12 * it under the terms of the GNU General Public License as published by
13 * the Free Software Foundation; either version 2 of the License, or
14 * (at your option) any later version.
16 * MPlayer is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU General Public License for more details.
21 * You should have received a copy of the GNU General Public License along
22 * with MPlayer; if not, write to the Free Software Foundation, Inc.,
23 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
26 #include <stdio.h>
27 #include <stdlib.h>
29 #include <inttypes.h>
30 #include <math.h>
32 #include "af.h"
34 #define L 2 // Storage for filter taps
35 #define KM 10 // Max number of bands
37 #define Q 1.2247449 /* Q value for band-pass filters 1.2247=(3/2)^(1/2)
38 gives 4dB suppression @ Fc*2 and Fc/2 */
40 /* Center frequencies for band-pass filters
41 The different frequency bands are:
42 nr. center frequency
43 0 31.25 Hz
44 1 62.50 Hz
45 2 125.0 Hz
46 3 250.0 Hz
47 4 500.0 Hz
48 5 1.000 kHz
49 6 2.000 kHz
50 7 4.000 kHz
51 8 8.000 kHz
52 9 16.00 kHz
54 #define CF {31.25,62.5,125,250,500,1000,2000,4000,8000,16000}
56 // Maximum and minimum gain for the bands
57 #define G_MAX +12.0
58 #define G_MIN -12.0
60 // Data for specific instances of this filter
61 typedef struct af_equalizer_s
63 float a[KM][L]; // A weights
64 float b[KM][L]; // B weights
65 float wq[AF_NCH][KM][L]; // Circular buffer for W data
66 float g[AF_NCH][KM]; // Gain factor for each channel and band
67 int K; // Number of used eq bands
68 int channels; // Number of channels
69 float gain_factor; // applied at output to avoid clipping
70 } af_equalizer_t;
72 // 2nd order Band-pass Filter design
73 static void bp2(float* a, float* b, float fc, float q){
74 double th= 2.0 * M_PI * fc;
75 double C = (1.0 - tan(th*q/2.0))/(1.0 + tan(th*q/2.0));
77 a[0] = (1.0 + C) * cos(th);
78 a[1] = -1 * C;
80 b[0] = (1.0 - C)/2.0;
81 b[1] = -1.0050;
84 // Initialization and runtime control
85 static int control(struct af_instance_s* af, int cmd, void* arg)
87 af_equalizer_t* s = (af_equalizer_t*)af->setup;
89 switch(cmd){
90 case AF_CONTROL_REINIT:{
91 int k =0, i =0;
92 float F[KM] = CF;
94 s->gain_factor=0.0;
96 // Sanity check
97 if(!arg) return AF_ERROR;
99 af->data->rate = ((af_data_t*)arg)->rate;
100 af->data->nch = ((af_data_t*)arg)->nch;
101 af->data->format = AF_FORMAT_FLOAT_NE;
102 af->data->bps = 4;
104 // Calculate number of active filters
105 s->K=KM;
106 while(F[s->K-1] > (float)af->data->rate/2.2)
107 s->K--;
109 if(s->K != KM)
110 mp_msg(MSGT_AFILTER, MSGL_INFO, "[equalizer] Limiting the number of filters to"
111 " %i due to low sample rate.\n",s->K);
113 // Generate filter taps
114 for(k=0;k<s->K;k++)
115 bp2(s->a[k],s->b[k],F[k]/((float)af->data->rate),Q);
117 // Calculate how much this plugin adds to the overall time delay
118 af->delay = 2 * af->data->nch * af->data->bps;
120 // Calculate gain factor to prevent clipping at output
121 for(k=0;k<AF_NCH;k++)
123 for(i=0;i<KM;i++)
125 if(s->gain_factor < s->g[k][i]) s->gain_factor=s->g[k][i];
129 s->gain_factor=log10(s->gain_factor + 1.0) * 20.0;
131 if(s->gain_factor > 0.0)
133 s->gain_factor=0.1+(s->gain_factor/12.0);
134 }else{
135 s->gain_factor=1;
138 return af_test_output(af,arg);
140 case AF_CONTROL_COMMAND_LINE:{
141 float g[10]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
142 int i,j;
143 sscanf((char*)arg,"%f:%f:%f:%f:%f:%f:%f:%f:%f:%f", &g[0], &g[1],
144 &g[2], &g[3], &g[4], &g[5], &g[6], &g[7], &g[8] ,&g[9]);
145 for(i=0;i<AF_NCH;i++){
146 for(j=0;j<KM;j++){
147 ((af_equalizer_t*)af->setup)->g[i][j] =
148 pow(10.0,clamp(g[j],G_MIN,G_MAX)/20.0)-1.0;
151 return AF_OK;
153 case AF_CONTROL_EQUALIZER_GAIN | AF_CONTROL_SET:{
154 float* gain = ((af_control_ext_t*)arg)->arg;
155 int ch = ((af_control_ext_t*)arg)->ch;
156 int k;
157 if(ch >= AF_NCH || ch < 0)
158 return AF_ERROR;
160 for(k = 0 ; k<KM ; k++)
161 s->g[ch][k] = pow(10.0,clamp(gain[k],G_MIN,G_MAX)/20.0)-1.0;
163 return AF_OK;
165 case AF_CONTROL_EQUALIZER_GAIN | AF_CONTROL_GET:{
166 float* gain = ((af_control_ext_t*)arg)->arg;
167 int ch = ((af_control_ext_t*)arg)->ch;
168 int k;
169 if(ch >= AF_NCH || ch < 0)
170 return AF_ERROR;
172 for(k = 0 ; k<KM ; k++)
173 gain[k] = log10(s->g[ch][k]+1.0) * 20.0;
175 return AF_OK;
178 return AF_UNKNOWN;
181 // Deallocate memory
182 static void uninit(struct af_instance_s* af)
184 free(af->data);
185 free(af->setup);
188 // Filter data through filter
189 static af_data_t* play(struct af_instance_s* af, af_data_t* data)
191 af_data_t* c = data; // Current working data
192 af_equalizer_t* s = (af_equalizer_t*)af->setup; // Setup
193 uint32_t ci = af->data->nch; // Index for channels
194 uint32_t nch = af->data->nch; // Number of channels
196 while(ci--){
197 float* g = s->g[ci]; // Gain factor
198 float* in = ((float*)c->audio)+ci;
199 float* out = ((float*)c->audio)+ci;
200 float* end = in + c->len/4; // Block loop end
202 while(in < end){
203 register int k = 0; // Frequency band index
204 register float yt = *in; // Current input sample
205 in+=nch;
207 // Run the filters
208 for(;k<s->K;k++){
209 // Pointer to circular buffer wq
210 register float* wq = s->wq[ci][k];
211 // Calculate output from AR part of current filter
212 register float w=yt*s->b[k][0] + wq[0]*s->a[k][0] + wq[1]*s->a[k][1];
213 // Calculate output form MA part of current filter
214 yt+=(w + wq[1]*s->b[k][1])*g[k];
215 // Update circular buffer
216 wq[1] = wq[0];
217 wq[0] = w;
219 // Calculate output
220 *out=yt*s->gain_factor;
221 out+=nch;
224 return c;
227 // Allocate memory and set function pointers
228 static int af_open(af_instance_t* af){
229 af->control=control;
230 af->uninit=uninit;
231 af->play=play;
232 af->mul=1;
233 af->data=calloc(1,sizeof(af_data_t));
234 af->setup=calloc(1,sizeof(af_equalizer_t));
235 if(af->data == NULL || af->setup == NULL)
236 return AF_ERROR;
237 return AF_OK;
240 // Description of this filter
241 af_info_t af_info_equalizer = {
242 "Equalizer audio filter",
243 "equalizer",
244 "Anders",
246 AF_FLAGS_NOT_REENTRANT,
247 af_open