8073
|
1 /*=============================================================================
|
|
2 //
|
|
3 // This software has been released under the terms of the GNU Public
|
|
4 // license. See http://www.gnu.org/copyleft/gpl.html for details.
|
|
5 //
|
|
6 // Copyright 2001 Anders Johansson ajh@atri.curtin.edu.au
|
|
7 //
|
|
8 //=============================================================================
|
|
9 */
|
|
10
|
|
11 /* Equalizer filter, implementation of a 10 band time domain graphic
|
|
12 equalizer using IIR filters. The IIR filters are implemented using a
|
|
13 Direct Form II approach, but has been modified (b1 == 0 always) to
|
|
14 save computation.
|
|
15 */
|
|
16
|
|
17 #include <stdio.h>
|
|
18 #include <stdlib.h>
|
|
19
|
|
20 #include <unistd.h>
|
|
21 #include <inttypes.h>
|
|
22 #include <math.h>
|
|
23
|
|
24 #include "af.h"
|
|
25 #include "equalizer.h"
|
|
26
|
8167
|
27 #define NCH AF_NCH // Number of channels
|
|
28 #define L 2 // Storage for filter taps
|
|
29 #define KM 10 // Max number of bands
|
8073
|
30
|
|
31 #define Q 1.2247449 /* Q value for band-pass filters 1.2247=(3/2)^(1/2)
|
|
32 gives 4dB suppression @ Fc*2 and Fc/2 */
|
|
33
|
|
34 // Center frequencies for band-pass filters
|
|
35 #define CF {31.25,62.5,125,250,500,1000,2000,4000,8000,16000}
|
|
36
|
|
37 // Maximum and minimum gain for the bands
|
|
38 #define G_MAX +12.0
|
|
39 #define G_MIN -12.0
|
|
40
|
|
41 // Data for specific instances of this filter
|
|
42 typedef struct af_equalizer_s
|
|
43 {
|
|
44 float a[KM][L]; // A weights
|
|
45 float b[KM][L]; // B weights
|
|
46 float wq[NCH][KM][L]; // Circular buffer for W data
|
|
47 float g[NCH][KM]; // Gain factor for each channel and band
|
|
48 int K; // Number of used eq bands
|
|
49 int channels; // Number of channels
|
|
50 } af_equalizer_t;
|
|
51
|
|
52 // 2nd order Band-pass Filter design
|
|
53 static void bp2(float* a, float* b, float fc, float q){
|
|
54 double th= 2.0 * M_PI * fc;
|
|
55 double C = (1.0 - tan(th*q/2.0))/(1.0 + tan(th*q/2.0));
|
|
56
|
|
57 a[0] = (1.0 + C) * cos(th);
|
|
58 a[1] = -1 * C;
|
|
59
|
|
60 b[0] = (1.0 - C)/2.0;
|
|
61 b[1] = -1.0050;
|
|
62 }
|
|
63
|
|
64 // Initialization and runtime control
|
|
65 static int control(struct af_instance_s* af, int cmd, void* arg)
|
|
66 {
|
|
67 af_equalizer_t* s = (af_equalizer_t*)af->setup;
|
|
68
|
|
69 switch(cmd){
|
|
70 case AF_CONTROL_REINIT:{
|
|
71 int k =0;
|
|
72 float F[KM] = CF;
|
|
73
|
|
74 // Sanity check
|
|
75 if(!arg) return AF_ERROR;
|
|
76
|
|
77 af->data->rate = ((af_data_t*)arg)->rate;
|
|
78 af->data->nch = ((af_data_t*)arg)->nch;
|
8167
|
79 af->data->format = AF_FORMAT_NE | AF_FORMAT_SI;
|
8073
|
80 af->data->bps = 2;
|
|
81
|
|
82 // Calculate number of active filters
|
|
83 s->K=KM;
|
8167
|
84 while(F[s->K-1] > (float)af->data->rate/2.2)
|
8073
|
85 s->K--;
|
8167
|
86
|
|
87 if(s->K != KM)
|
|
88 af_msg(AF_MSG_INFO,"Limiting the number of filters to %i due to low sample rate.\n",s->K);
|
8073
|
89
|
|
90 // Generate filter taps
|
|
91 for(k=0;k<s->K;k++)
|
|
92 bp2(s->a[k],s->b[k],F[k]/((float)af->data->rate),Q);
|
|
93
|
|
94 // Calculate how much this plugin adds to the overall time delay
|
|
95 af->delay += 2000.0/((float)af->data->rate);
|
|
96
|
8167
|
97 // Only signed 16 bit little endian is supported
|
8073
|
98 if(af->data->format != ((af_data_t*)arg)->format ||
|
|
99 af->data->bps != ((af_data_t*)arg)->bps)
|
|
100 return AF_FALSE;
|
|
101 return AF_OK;
|
|
102 }
|
|
103 case AF_CONTROL_COMMAND_LINE:{
|
|
104 float g[10]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
|
|
105 int i,j;
|
|
106 sscanf((char*)arg,"%f:%f:%f:%f:%f:%f:%f:%f:%f:%f", &g[0], &g[1],
|
|
107 &g[2], &g[3], &g[4], &g[5], &g[6], &g[7], &g[8] ,&g[9]);
|
|
108 for(i=0;i<NCH;i++){
|
|
109 for(j=0;j<KM;j++){
|
|
110 ((af_equalizer_t*)af->setup)->g[i][j] =
|
|
111 pow(10.0,clamp(g[j],G_MIN,G_MAX)/20.0)-1.0;
|
|
112 }
|
|
113 }
|
|
114 return AF_OK;
|
|
115 }
|
|
116 case AF_CONTROL_EQUALIZER_SET_GAIN:{
|
|
117 float gain = ((equalizer_t*)arg)->gain;
|
|
118 int ch = ((equalizer_t*)arg)->channel;
|
|
119 int band = ((equalizer_t*)arg)->band;
|
|
120 if(ch > NCH || ch < 0 || band > KM || band < 0)
|
|
121 return AF_ERROR;
|
|
122
|
|
123 s->g[ch][band] = pow(10.0,clamp(gain,G_MIN,G_MAX)/20.0)-1.0;
|
|
124 return AF_OK;
|
|
125 }
|
|
126 case AF_CONTROL_EQUALIZER_GET_GAIN:{
|
|
127 int ch =((equalizer_t*)arg)->channel;
|
|
128 int band =((equalizer_t*)arg)->band;
|
|
129 if(ch > NCH || ch < 0 || band > KM || band < 0)
|
|
130 return AF_ERROR;
|
|
131
|
|
132 ((equalizer_t*)arg)->gain = log10(s->g[ch][band]+1.0) * 20.0;
|
|
133 return AF_OK;
|
|
134 }
|
|
135 }
|
|
136 return AF_UNKNOWN;
|
|
137 }
|
|
138
|
|
139 // Deallocate memory
|
|
140 static void uninit(struct af_instance_s* af)
|
|
141 {
|
|
142 if(af->data)
|
|
143 free(af->data);
|
|
144 if(af->setup)
|
|
145 free(af->setup);
|
|
146 }
|
|
147
|
|
148 // Filter data through filter
|
|
149 static af_data_t* play(struct af_instance_s* af, af_data_t* data)
|
|
150 {
|
|
151 af_data_t* c = data; // Current working data
|
|
152 af_equalizer_t* s = (af_equalizer_t*)af->setup; // Setup
|
|
153 uint32_t ci = af->data->nch; // Index for channels
|
|
154 uint32_t nch = af->data->nch; // Number of channels
|
|
155
|
|
156 while(ci--){
|
|
157 float* g = s->g[ci]; // Gain factor
|
|
158 int16_t* in = ((int16_t*)c->audio)+ci;
|
|
159 int16_t* out = ((int16_t*)c->audio)+ci;
|
|
160 int16_t* end = in + c->len/2; // Block loop end
|
|
161
|
|
162 while(in < end){
|
|
163 register uint32_t k = 0; // Frequency band index
|
|
164 register float yt = (float)(*in); // Current input sample
|
|
165 in+=nch;
|
|
166
|
|
167 // Run the filters
|
|
168 for(;k<s->K;k++){
|
|
169 // Pointer to circular buffer wq
|
|
170 register float* wq = s->wq[ci][k];
|
|
171 // Calculate output from AR part of current filter
|
|
172 register float w=yt*s->b[k][0] + wq[0]*s->a[k][0] + wq[1]*s->a[k][1];
|
|
173 // Calculate output form MA part of current filter
|
|
174 yt+=(w + wq[1]*s->b[k][1])*g[k];
|
|
175 // Update circular buffer
|
|
176 wq[1] = wq[0];
|
|
177 wq[0] = w;
|
|
178 }
|
|
179 // Calculate output
|
|
180 *out=(int16_t)(yt/(4.0*10.0));
|
|
181 out+=nch;
|
|
182 }
|
|
183 }
|
|
184 return c;
|
|
185 }
|
|
186
|
|
187 // Allocate memory and set function pointers
|
|
188 static int open(af_instance_t* af){
|
|
189 af->control=control;
|
|
190 af->uninit=uninit;
|
|
191 af->play=play;
|
|
192 af->mul.n=1;
|
|
193 af->mul.d=1;
|
|
194 af->data=calloc(1,sizeof(af_data_t));
|
|
195 af->setup=calloc(1,sizeof(af_equalizer_t));
|
|
196 if(af->data == NULL || af->setup == NULL)
|
|
197 return AF_ERROR;
|
|
198 return AF_OK;
|
|
199 }
|
|
200
|
|
201 // Description of this filter
|
|
202 af_info_t af_info_equalizer = {
|
|
203 "Equalizer audio filter",
|
|
204 "equalizer",
|
|
205 "Anders",
|
|
206 "",
|
|
207 AF_FLAGS_NOT_REENTRANT,
|
|
208 open
|
|
209 };
|
|
210
|
|
211
|
|
212
|
|
213
|
|
214
|
|
215
|
|
216
|