6430
|
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 plugin, 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 #define PLUGIN
|
|
17
|
|
18 #include <stdio.h>
|
|
19 #include <stdlib.h>
|
|
20 #include <unistd.h>
|
|
21 #include <inttypes.h>
|
|
22 #include <math.h>
|
|
23
|
|
24 #include "audio_out.h"
|
|
25 #include "audio_plugin.h"
|
|
26 #include "audio_plugin_internal.h"
|
|
27 #include "afmt.h"
|
|
28 #include "eq.h"
|
|
29
|
|
30 static ao_info_t info =
|
|
31 {
|
|
32 "Equalizer audio plugin",
|
|
33 "eq",
|
|
34 "Anders",
|
|
35 ""
|
|
36 };
|
|
37
|
|
38 LIBAO_PLUGIN_EXTERN(eq)
|
|
39
|
|
40
|
|
41 #define CH 6 // Max number of channels
|
|
42 #define L 2 // Storage for filter taps
|
|
43 #define KM 10 // Max number of octaves
|
|
44
|
|
45 #define Q 1.2247 /* Q value for band-pass filters 1.2247=(3/2)^(1/2)
|
|
46 gives 4db suppression @ Fc*2 and Fc/2 */
|
|
47
|
|
48 // Center frequencies for band-pass filters
|
|
49 #define CF {31.25,62.5,125,250,500,1000,2000,4000,8000,16000}
|
|
50
|
|
51 // local data
|
|
52 typedef struct pl_eq_s
|
|
53 {
|
|
54 int16_t a[KM][L]; // A weights
|
|
55 int16_t b[KM][L]; // B weights
|
|
56 int16_t wq[CH][KM][L]; // Circular buffer for W data
|
|
57 int16_t g[CH][KM]; // Gain factor for each channel and band
|
|
58 int16_t K; // Number of used eq bands
|
|
59 int channels; // Number of channels
|
|
60 } pl_eq_t;
|
|
61
|
|
62 static pl_eq_t pl_eq;
|
|
63
|
|
64 // to set/get/query special features/parameters
|
|
65 static int control(int cmd,int arg){
|
|
66 switch(cmd){
|
|
67 case AOCONTROL_PLUGIN_SET_LEN:
|
|
68 return CONTROL_OK;
|
|
69 case AOCONTROL_PLUGIN_EQ_SET_GAIN:{
|
|
70 float gain = ((equalizer_t*)arg)->gain;
|
|
71 int ch =((equalizer_t*)arg)->channel;
|
|
72 int band =((equalizer_t*)arg)->band;
|
|
73 if(ch > CH || ch < 0 || band > KM || band < 0)
|
|
74 return CONTROL_ERROR;
|
|
75
|
|
76 pl_eq.g[ch][band]=(int16_t) 4096 * (pow(10.0,gain/20.0)-1.0);
|
|
77 return CONTROL_OK;
|
|
78 }
|
|
79 case AOCONTROL_PLUGIN_EQ_GET_GAIN:{
|
|
80 int ch =((equalizer_t*)arg)->channel;
|
|
81 int band =((equalizer_t*)arg)->band;
|
|
82 if(ch > CH || ch < 0 || band > KM || band < 0)
|
|
83 return CONTROL_ERROR;
|
|
84
|
|
85 ((equalizer_t*)arg)->gain = log10((float)pl_eq.g[ch][band]/4096.0+1) * 20.0;
|
|
86 return CONTROL_OK;
|
|
87 }
|
|
88 }
|
|
89 return CONTROL_UNKNOWN;
|
|
90 }
|
|
91
|
|
92 // 2nd order Band-pass Filter design
|
|
93 void bp2(int16_t* a, int16_t* b, float fc, float q){
|
|
94 double th=2*3.141592654*fc;
|
|
95 double C=(1 - tan(th*q/2))/(1 + tan(th*q/2));
|
|
96
|
|
97 a[0] = (int16_t)lround( 16383.0 * (1 + C) * cos(th));
|
|
98 a[1] = (int16_t)lround(-16383.0 * C);
|
|
99
|
|
100 b[0] = (int16_t)lround(-16383.0 * (C - 1)/2);
|
|
101 b[1] = (int16_t)lround(-16383.0 * 1.0050);
|
|
102 }
|
|
103
|
|
104 // empty buffers
|
|
105 static void reset(){
|
|
106 int k,l,c;
|
|
107 for(c=0;c<pl_eq.channels;c++)
|
|
108 for(k=0;k<pl_eq.K;k++)
|
|
109 for(l=0;l<L*2;l++)
|
|
110 pl_eq.wq[c][k][l]=0;
|
|
111 }
|
|
112
|
|
113 // open & setup audio device
|
|
114 // return: 1=success 0=fail
|
|
115 static int init(){
|
|
116 int c,k = 0;
|
|
117 float F[KM] = CF;
|
|
118
|
|
119 // Check input format
|
|
120 if(ao_plugin_data.format != AFMT_S16_LE){
|
|
121 fprintf(stderr,"[pl_eq] Input audio format not yet supported. \n");
|
|
122 return 0;
|
|
123 }
|
|
124
|
|
125 // Check number of channels
|
|
126 if(ao_plugin_data.channels>CH){
|
|
127 fprintf(stderr,"[pl_eq] Too many channels, max is 6.\n");
|
|
128 return 0;
|
|
129 }
|
|
130 pl_eq.channels=ao_plugin_data.channels;
|
|
131
|
|
132 // Calculate number of active filters
|
|
133 pl_eq.K=KM;
|
|
134 while(F[pl_eq.K-1] > (float)ao_plugin_data.rate/2)
|
|
135 pl_eq.K--;
|
|
136
|
|
137 // Generate filter taps
|
|
138 for(k=0;k<pl_eq.K;k++)
|
|
139 bp2(pl_eq.a[k],pl_eq.b[k],F[k]/((float)ao_plugin_data.rate),Q);
|
|
140
|
|
141 // Reset buffers
|
|
142 reset();
|
|
143
|
|
144 // Reset gain factors
|
|
145 for(c=0;c<pl_eq.channels;c++)
|
|
146 for(k=0;k<pl_eq.K;k++)
|
|
147 pl_eq.g[c][k]=0;
|
|
148
|
|
149 // Tell ao_plugin how much this plugin adds to the overall time delay
|
|
150 ao_plugin_data.delay_fix-=2/((float)pl_eq.channels*(float)ao_plugin_data.rate);
|
|
151 // Print some cool remark of what the plugin does
|
|
152 printf("[pl_eq] Equalizer in use.\n");
|
|
153 return 1;
|
|
154 }
|
|
155
|
|
156 // close plugin
|
|
157 static void uninit(){
|
|
158 }
|
|
159
|
|
160 // processes 'ao_plugin_data.len' bytes of 'data'
|
|
161 // called for every block of data
|
|
162 static int play(){
|
|
163 uint16_t ci = pl_eq.channels; // Index for channels
|
|
164 uint16_t nch = pl_eq.channels; // Number of channels
|
|
165
|
|
166 while(ci--){
|
|
167 int16_t* g = pl_eq.g[ci]; // Gain factor
|
|
168 int16_t* in = ((int16_t*)ao_plugin_data.data)+ci;
|
|
169 int16_t* out = ((int16_t*)ao_plugin_data.data)+ci;
|
|
170 int16_t* end = in+ao_plugin_data.len/2; // Block loop end
|
|
171
|
|
172 while(in < end){
|
|
173 register int16_t k = 0; // Frequency band index
|
|
174 register int32_t yt = 0; // Total output from filters
|
|
175 register int16_t x = *in; /* Current input sample scale
|
|
176 to prevent overflow in wq */
|
|
177 in+=nch;
|
|
178
|
|
179 // Run the filters
|
|
180 for(;k<pl_eq.K;k++){
|
|
181 // Pointer to circular buffer wq
|
|
182 register int16_t* wq = pl_eq.wq[ci][k];
|
|
183 // Calculate output from AR part of current filter
|
|
184 register int32_t xt = (x*pl_eq.b[k][0]) >> 4;
|
|
185 register int32_t w = xt + wq[0]*pl_eq.a[k][0] + wq[1]*pl_eq.a[k][1];
|
|
186 // Calculate output form MA part of current filter
|
|
187 yt+=(((w + wq[1]*pl_eq.b[k][1]) >> 10)*g[k]) >> 12;
|
|
188 // Update circular buffer
|
|
189 wq[1] = wq[0]; wq[0] = w >> 14;
|
|
190 }
|
|
191
|
|
192 // Calculate output
|
|
193 *out=(int16_t)(yt+x);
|
|
194 out+=nch;
|
|
195 }
|
|
196 }
|
|
197 return 1;
|
|
198 }
|
|
199
|
|
200
|
|
201
|
|
202
|
|
203
|
|
204
|
|
205
|