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