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
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
|
/***************************************************************************
* __________ __ ___.
* Open \______ \ ____ ____ | | _\_ |__ _______ ___
* Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
* Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
* Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
* \/ \/ \/ \/ \/
* $Id$
*
* Copyright (C) 2006-2007 Thom Johansen
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version.
*
* This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
* KIND, either express or implied.
*
****************************************************************************/
#include <inttypes.h>
#include "config.h"
#include "fixedpoint.h"
#include "fracmul.h"
#include "eq.h"
#include "replaygain.h"
/**
* Calculate first order shelving filter. Filter is not directly usable by the
* eq_filter() function.
* @param cutoff shelf midpoint frequency. See eq_pk_coefs for format.
* @param A decibel value multiplied by ten, describing gain/attenuation of
* shelf. Max value is 24 dB.
* @param low true for low-shelf filter, false for high-shelf filter.
* @param c pointer to coefficient storage. Coefficients are s4.27 format.
*/
void filter_shelf_coefs(unsigned long cutoff, long A, bool low, int32_t *c)
{
long sin, cos;
int32_t b0, b1, a0, a1; /* s3.28 */
const long g = get_replaygain_int(A*5) << 4; /* 10^(db/40), s3.28 */
sin = fp_sincos(cutoff/2, &cos);
if (low) {
const int32_t sin_div_g = fp_div(sin, g, 25);
const int32_t sin_g = FRACMUL(sin, g);
cos >>= 3;
b0 = sin_g + cos; /* 0.25 .. 4.10 */
b1 = sin_g - cos; /* -1 .. 3.98 */
a0 = sin_div_g + cos; /* 0.25 .. 4.10 */
a1 = sin_div_g - cos; /* -1 .. 3.98 */
} else {
const int32_t cos_div_g = fp_div(cos, g, 25);
const int32_t cos_g = FRACMUL(cos, g);
sin >>= 3;
b0 = sin + cos_g; /* 0.25 .. 4.10 */
b1 = sin - cos_g; /* -3.98 .. 1 */
a0 = sin + cos_div_g; /* 0.25 .. 4.10 */
a1 = sin - cos_div_g; /* -3.98 .. 1 */
}
const int32_t rcp_a0 = fp_div(1, a0, 57); /* 0.24 .. 3.98, s2.29 */
*c++ = FRACMUL_SHL(b0, rcp_a0, 1); /* 0.063 .. 15.85 */
*c++ = FRACMUL_SHL(b1, rcp_a0, 1); /* -15.85 .. 15.85 */
*c++ = -FRACMUL_SHL(a1, rcp_a0, 1); /* -1 .. 1 */
}
#ifdef HAVE_SW_TONE_CONTROLS
/**
* Calculate second order section filter consisting of one low-shelf and one
* high-shelf section.
* @param cutoff_low low-shelf midpoint frequency. See eq_pk_coefs for format.
* @param cutoff_high high-shelf midpoint frequency.
* @param A_low decibel value multiplied by ten, describing gain/attenuation of
* low-shelf part. Max value is 24 dB.
* @param A_high decibel value multiplied by ten, describing gain/attenuation of
* high-shelf part. Max value is 24 dB.
* @param A decibel value multiplied by ten, describing additional overall gain.
* @param c pointer to coefficient storage. Coefficients are s4.27 format.
*/
void filter_bishelf_coefs(unsigned long cutoff_low, unsigned long cutoff_high,
long A_low, long A_high, long A, int32_t *c)
{
const long g = get_replaygain_int(A*10) << 7; /* 10^(db/20), s0.31 */
int32_t c_ls[3], c_hs[3];
filter_shelf_coefs(cutoff_low, A_low, true, c_ls);
filter_shelf_coefs(cutoff_high, A_high, false, c_hs);
c_ls[0] = FRACMUL(g, c_ls[0]);
c_ls[1] = FRACMUL(g, c_ls[1]);
/* now we cascade the two first order filters to one second order filter
* which can be used by eq_filter(). these resulting coefficients have a
* really wide numerical range, so we use a fixed point format which will
* work for the selected cutoff frequencies (in dsp.c) only.
*/
const int32_t b0 = c_ls[0], b1 = c_ls[1], b2 = c_hs[0], b3 = c_hs[1];
const int32_t a0 = c_ls[2], a1 = c_hs[2];
*c++ = FRACMUL_SHL(b0, b2, 4);
*c++ = FRACMUL_SHL(b0, b3, 4) + FRACMUL_SHL(b1, b2, 4);
*c++ = FRACMUL_SHL(b1, b3, 4);
*c++ = a0 + a1;
*c++ = -FRACMUL_SHL(a0, a1, 4);
}
#endif
/* Coef calculation taken from Audio-EQ-Cookbook.txt by Robert Bristow-Johnson.
* Slightly faster calculation can be done by deriving forms which use tan()
* instead of cos() and sin(), but the latter are far easier to use when doing
* fixed point math, and performance is not a big point in the calculation part.
* All the 'a' filter coefficients are negated so we can use only additions
* in the filtering equation.
*/
/**
* Calculate second order section peaking filter coefficients.
* @param cutoff a value from 0 to 0x80000000, where 0 represents 0 Hz and
* 0x80000000 represents the Nyquist frequency (samplerate/2).
* @param Q Q factor value multiplied by ten. Lower bound is artificially set
* at 0.5.
* @param db decibel value multiplied by ten, describing gain/attenuation at
* peak freq. Max value is 24 dB.
* @param c pointer to coefficient storage. Coefficients are s3.28 format.
*/
void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c)
{
long cs;
const long one = 1 << 28; /* s3.28 */
const long A = get_replaygain_int(db*5) << 5; /* 10^(db/40), s2.29 */
const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */
int32_t a0, a1, a2; /* these are all s3.28 format */
int32_t b0, b1, b2;
const long alphadivA = fp_div(alpha, A, 27);
const long alphaA = FRACMUL(alpha, A);
/* possible numerical ranges are in comments by each coef */
b0 = one + alphaA; /* [1 .. 5] */
b1 = a1 = -2*(cs >> 3); /* [-2 .. 2] */
b2 = one - alphaA; /* [-3 .. 1] */
a0 = one + alphadivA; /* [1 .. 5] */
a2 = one - alphadivA; /* [-3 .. 1] */
/* range of this is roughly [0.2 .. 1], but we'll never hit 1 completely */
const long rcp_a0 = fp_div(1, a0, 59); /* s0.31 */
*c++ = FRACMUL(b0, rcp_a0); /* [0.25 .. 4] */
*c++ = FRACMUL(b1, rcp_a0); /* [-2 .. 2] */
*c++ = FRACMUL(b2, rcp_a0); /* [-2.4 .. 1] */
*c++ = FRACMUL(-a1, rcp_a0); /* [-2 .. 2] */
*c++ = FRACMUL(-a2, rcp_a0); /* [-0.6 .. 1] */
}
/**
* Calculate coefficients for lowshelf filter. Parameters are as for
* eq_pk_coefs, but the coefficient format is s5.26 fixed point.
*/
void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c)
{
long cs;
const long one = 1 << 25; /* s6.25 */
const long sqrtA = get_replaygain_int(db*5/2) << 2; /* 10^(db/80), s5.26 */
const long A = FRACMUL_SHL(sqrtA, sqrtA, 8); /* s2.29 */
const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */
const long ap1 = (A >> 4) + one;
const long am1 = (A >> 4) - one;
const long ap1_cs = FRACMUL(ap1, cs);
const long am1_cs = FRACMUL(am1, cs);
const long twosqrtalpha = 2*FRACMUL(sqrtA, alpha);
int32_t a0, a1, a2; /* these are all s6.25 format */
int32_t b0, b1, b2;
/* [0.1 .. 40] */
b0 = FRACMUL_SHL(A, ap1 - am1_cs + twosqrtalpha, 2);
/* [-16 .. 63.4] */
b1 = FRACMUL_SHL(A, am1 - ap1_cs, 3);
/* [0 .. 31.7] */
b2 = FRACMUL_SHL(A, ap1 - am1_cs - twosqrtalpha, 2);
/* [0.5 .. 10] */
a0 = ap1 + am1_cs + twosqrtalpha;
/* [-16 .. 4] */
a1 = -2*(am1 + ap1_cs);
/* [0 .. 8] */
a2 = ap1 + am1_cs - twosqrtalpha;
/* [0.1 .. 1.99] */
const long rcp_a0 = fp_div(1, a0, 55); /* s1.30 */
*c++ = FRACMUL_SHL(b0, rcp_a0, 2); /* [0.06 .. 15.9] */
*c++ = FRACMUL_SHL(b1, rcp_a0, 2); /* [-2 .. 31.7] */
*c++ = FRACMUL_SHL(b2, rcp_a0, 2); /* [0 .. 15.9] */
*c++ = FRACMUL_SHL(-a1, rcp_a0, 2); /* [-2 .. 2] */
*c++ = FRACMUL_SHL(-a2, rcp_a0, 2); /* [0 .. 1] */
}
/**
* Calculate coefficients for highshelf filter. Parameters are as for
* eq_pk_coefs, but the coefficient format is s5.26 fixed point.
*/
void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c)
{
long cs;
const long one = 1 << 25; /* s6.25 */
const long sqrtA = get_replaygain_int(db*5/2) << 2; /* 10^(db/80), s5.26 */
const long A = FRACMUL_SHL(sqrtA, sqrtA, 8); /* s2.29 */
const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */
const long ap1 = (A >> 4) + one;
const long am1 = (A >> 4) - one;
const long ap1_cs = FRACMUL(ap1, cs);
const long am1_cs = FRACMUL(am1, cs);
const long twosqrtalpha = 2*FRACMUL(sqrtA, alpha);
int32_t a0, a1, a2; /* these are all s6.25 format */
int32_t b0, b1, b2;
/* [0.1 .. 40] */
b0 = FRACMUL_SHL(A, ap1 + am1_cs + twosqrtalpha, 2);
/* [-63.5 .. 16] */
b1 = -FRACMUL_SHL(A, am1 + ap1_cs, 3);
/* [0 .. 32] */
b2 = FRACMUL_SHL(A, ap1 + am1_cs - twosqrtalpha, 2);
/* [0.5 .. 10] */
a0 = ap1 - am1_cs + twosqrtalpha;
/* [-4 .. 16] */
a1 = 2*(am1 - ap1_cs);
/* [0 .. 8] */
a2 = ap1 - am1_cs - twosqrtalpha;
/* [0.1 .. 1.99] */
const long rcp_a0 = fp_div(1, a0, 55); /* s1.30 */
*c++ = FRACMUL_SHL(b0, rcp_a0, 2); /* [0 .. 16] */
*c++ = FRACMUL_SHL(b1, rcp_a0, 2); /* [-31.7 .. 2] */
*c++ = FRACMUL_SHL(b2, rcp_a0, 2); /* [0 .. 16] */
*c++ = FRACMUL_SHL(-a1, rcp_a0, 2); /* [-2 .. 2] */
*c++ = FRACMUL_SHL(-a2, rcp_a0, 2); /* [0 .. 1] */
}
/* We realise the filters as a second order direct form 1 structure. Direct
* form 1 was chosen because of better numerical properties for fixed point
* implementations.
*/
#if (!defined(CPU_COLDFIRE) && !defined(CPU_ARM))
void eq_filter(int32_t **x, struct eqfilter *f, unsigned num,
unsigned channels, unsigned shift)
{
unsigned c, i;
long long acc;
/* Direct form 1 filtering code.
y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2],
where y[] is output and x[] is input.
*/
for (c = 0; c < channels; c++) {
for (i = 0; i < num; i++) {
acc = (long long) x[c][i] * f->coefs[0];
acc += (long long) f->history[c][0] * f->coefs[1];
acc += (long long) f->history[c][1] * f->coefs[2];
acc += (long long) f->history[c][2] * f->coefs[3];
acc += (long long) f->history[c][3] * f->coefs[4];
f->history[c][1] = f->history[c][0];
f->history[c][0] = x[c][i];
f->history[c][3] = f->history[c][2];
x[c][i] = (acc << shift) >> 32;
f->history[c][2] = x[c][i];
}
}
}
#endif
|
