1 /* Copyright (c) 2013 The Chromium OS Authors. All rights reserved.
2 * Use of this source code is governed by a BSD-style license that can be
3 * found in the LICENSE file.
4 */
5
6 /* Copyright (C) 2010 Google Inc. All rights reserved.
7 * Use of this source code is governed by a BSD-style license that can be
8 * found in the LICENSE.WEBKIT file.
9 */
10
11 #include <math.h>
12 #include "biquad.h"
13
14 #ifndef max
15 #define max(a, b) ({ __typeof__(a) _a = (a); \
16 __typeof__(b) _b = (b); \
17 _a > _b ? _a : _b; })
18 #endif
19
20 #ifndef min
21 #define min(a, b) ({ __typeof__(a) _a = (a); \
22 __typeof__(b) _b = (b); \
23 _a < _b ? _a : _b; })
24 #endif
25
26 #ifndef M_PI
27 #define M_PI 3.14159265358979323846
28 #endif
29
set_coefficient(struct biquad * bq,double b0,double b1,double b2,double a0,double a1,double a2)30 static void set_coefficient(struct biquad *bq, double b0, double b1, double b2,
31 double a0, double a1, double a2)
32 {
33 double a0_inv = 1 / a0;
34 bq->b0 = b0 * a0_inv;
35 bq->b1 = b1 * a0_inv;
36 bq->b2 = b2 * a0_inv;
37 bq->a1 = a1 * a0_inv;
38 bq->a2 = a2 * a0_inv;
39 }
40
biquad_lowpass(struct biquad * bq,double cutoff,double resonance)41 static void biquad_lowpass(struct biquad *bq, double cutoff, double resonance)
42 {
43 /* Limit cutoff to 0 to 1. */
44 cutoff = max(0.0, min(cutoff, 1.0));
45
46 if (cutoff == 1) {
47 /* When cutoff is 1, the z-transform is 1. */
48 set_coefficient(bq, 1, 0, 0, 1, 0, 0);
49 } else if (cutoff > 0) {
50 /* Compute biquad coefficients for lowpass filter */
51 resonance = max(0.0, resonance); /* can't go negative */
52 double g = pow(10.0, 0.05 * resonance);
53 double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2);
54
55 double theta = M_PI * cutoff;
56 double sn = 0.5 * d * sin(theta);
57 double beta = 0.5 * (1 - sn) / (1 + sn);
58 double gamma = (0.5 + beta) * cos(theta);
59 double alpha = 0.25 * (0.5 + beta - gamma);
60
61 double b0 = 2 * alpha;
62 double b1 = 2 * 2 * alpha;
63 double b2 = 2 * alpha;
64 double a1 = 2 * -gamma;
65 double a2 = 2 * beta;
66
67 set_coefficient(bq, b0, b1, b2, 1, a1, a2);
68 } else {
69 /* When cutoff is zero, nothing gets through the filter, so set
70 * coefficients up correctly.
71 */
72 set_coefficient(bq, 0, 0, 0, 1, 0, 0);
73 }
74 }
75
biquad_highpass(struct biquad * bq,double cutoff,double resonance)76 static void biquad_highpass(struct biquad *bq, double cutoff, double resonance)
77 {
78 /* Limit cutoff to 0 to 1. */
79 cutoff = max(0.0, min(cutoff, 1.0));
80
81 if (cutoff == 1) {
82 /* The z-transform is 0. */
83 set_coefficient(bq, 0, 0, 0, 1, 0, 0);
84 } else if (cutoff > 0) {
85 /* Compute biquad coefficients for highpass filter */
86 resonance = max(0.0, resonance); /* can't go negative */
87 double g = pow(10.0, 0.05 * resonance);
88 double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2);
89
90 double theta = M_PI * cutoff;
91 double sn = 0.5 * d * sin(theta);
92 double beta = 0.5 * (1 - sn) / (1 + sn);
93 double gamma = (0.5 + beta) * cos(theta);
94 double alpha = 0.25 * (0.5 + beta + gamma);
95
96 double b0 = 2 * alpha;
97 double b1 = 2 * -2 * alpha;
98 double b2 = 2 * alpha;
99 double a1 = 2 * -gamma;
100 double a2 = 2 * beta;
101
102 set_coefficient(bq, b0, b1, b2, 1, a1, a2);
103 } else {
104 /* When cutoff is zero, we need to be careful because the above
105 * gives a quadratic divided by the same quadratic, with poles
106 * and zeros on the unit circle in the same place. When cutoff
107 * is zero, the z-transform is 1.
108 */
109 set_coefficient(bq, 1, 0, 0, 1, 0, 0);
110 }
111 }
112
biquad_bandpass(struct biquad * bq,double frequency,double Q)113 static void biquad_bandpass(struct biquad *bq, double frequency, double Q)
114 {
115 /* No negative frequencies allowed. */
116 frequency = max(0.0, frequency);
117
118 /* Don't let Q go negative, which causes an unstable filter. */
119 Q = max(0.0, Q);
120
121 if (frequency > 0 && frequency < 1) {
122 double w0 = M_PI * frequency;
123 if (Q > 0) {
124 double alpha = sin(w0) / (2 * Q);
125 double k = cos(w0);
126
127 double b0 = alpha;
128 double b1 = 0;
129 double b2 = -alpha;
130 double a0 = 1 + alpha;
131 double a1 = -2 * k;
132 double a2 = 1 - alpha;
133
134 set_coefficient(bq, b0, b1, b2, a0, a1, a2);
135 } else {
136 /* When Q = 0, the above formulas have problems. If we
137 * look at the z-transform, we can see that the limit
138 * as Q->0 is 1, so set the filter that way.
139 */
140 set_coefficient(bq, 1, 0, 0, 1, 0, 0);
141 }
142 } else {
143 /* When the cutoff is zero, the z-transform approaches 0, if Q
144 * > 0. When both Q and cutoff are zero, the z-transform is
145 * pretty much undefined. What should we do in this case?
146 * For now, just make the filter 0. When the cutoff is 1, the
147 * z-transform also approaches 0.
148 */
149 set_coefficient(bq, 0, 0, 0, 1, 0, 0);
150 }
151 }
152
biquad_lowshelf(struct biquad * bq,double frequency,double db_gain)153 static void biquad_lowshelf(struct biquad *bq, double frequency, double db_gain)
154 {
155 /* Clip frequencies to between 0 and 1, inclusive. */
156 frequency = max(0.0, min(frequency, 1.0));
157
158 double A = pow(10.0, db_gain / 40);
159
160 if (frequency == 1) {
161 /* The z-transform is a constant gain. */
162 set_coefficient(bq, A * A, 0, 0, 1, 0, 0);
163 } else if (frequency > 0) {
164 double w0 = M_PI * frequency;
165 double S = 1; /* filter slope (1 is max value) */
166 double alpha = 0.5 * sin(w0) *
167 sqrt((A + 1 / A) * (1 / S - 1) + 2);
168 double k = cos(w0);
169 double k2 = 2 * sqrt(A) * alpha;
170 double a_plus_one = A + 1;
171 double a_minus_one = A - 1;
172
173 double b0 = A * (a_plus_one - a_minus_one * k + k2);
174 double b1 = 2 * A * (a_minus_one - a_plus_one * k);
175 double b2 = A * (a_plus_one - a_minus_one * k - k2);
176 double a0 = a_plus_one + a_minus_one * k + k2;
177 double a1 = -2 * (a_minus_one + a_plus_one * k);
178 double a2 = a_plus_one + a_minus_one * k - k2;
179
180 set_coefficient(bq, b0, b1, b2, a0, a1, a2);
181 } else {
182 /* When frequency is 0, the z-transform is 1. */
183 set_coefficient(bq, 1, 0, 0, 1, 0, 0);
184 }
185 }
186
biquad_highshelf(struct biquad * bq,double frequency,double db_gain)187 static void biquad_highshelf(struct biquad *bq, double frequency,
188 double db_gain)
189 {
190 /* Clip frequencies to between 0 and 1, inclusive. */
191 frequency = max(0.0, min(frequency, 1.0));
192
193 double A = pow(10.0, db_gain / 40);
194
195 if (frequency == 1) {
196 /* The z-transform is 1. */
197 set_coefficient(bq, 1, 0, 0, 1, 0, 0);
198 } else if (frequency > 0) {
199 double w0 = M_PI * frequency;
200 double S = 1; /* filter slope (1 is max value) */
201 double alpha = 0.5 * sin(w0) *
202 sqrt((A + 1 / A) * (1 / S - 1) + 2);
203 double k = cos(w0);
204 double k2 = 2 * sqrt(A) * alpha;
205 double a_plus_one = A + 1;
206 double a_minus_one = A - 1;
207
208 double b0 = A * (a_plus_one + a_minus_one * k + k2);
209 double b1 = -2 * A * (a_minus_one + a_plus_one * k);
210 double b2 = A * (a_plus_one + a_minus_one * k - k2);
211 double a0 = a_plus_one - a_minus_one * k + k2;
212 double a1 = 2 * (a_minus_one - a_plus_one * k);
213 double a2 = a_plus_one - a_minus_one * k - k2;
214
215 set_coefficient(bq, b0, b1, b2, a0, a1, a2);
216 } else {
217 /* When frequency = 0, the filter is just a gain, A^2. */
218 set_coefficient(bq, A * A, 0, 0, 1, 0, 0);
219 }
220 }
221
biquad_peaking(struct biquad * bq,double frequency,double Q,double db_gain)222 static void biquad_peaking(struct biquad *bq, double frequency, double Q,
223 double db_gain)
224 {
225 /* Clip frequencies to between 0 and 1, inclusive. */
226 frequency = max(0.0, min(frequency, 1.0));
227
228 /* Don't let Q go negative, which causes an unstable filter. */
229 Q = max(0.0, Q);
230
231 double A = pow(10.0, db_gain / 40);
232
233 if (frequency > 0 && frequency < 1) {
234 if (Q > 0) {
235 double w0 = M_PI * frequency;
236 double alpha = sin(w0) / (2 * Q);
237 double k = cos(w0);
238
239 double b0 = 1 + alpha * A;
240 double b1 = -2 * k;
241 double b2 = 1 - alpha * A;
242 double a0 = 1 + alpha / A;
243 double a1 = -2 * k;
244 double a2 = 1 - alpha / A;
245
246 set_coefficient(bq, b0, b1, b2, a0, a1, a2);
247 } else {
248 /* When Q = 0, the above formulas have problems. If we
249 * look at the z-transform, we can see that the limit
250 * as Q->0 is A^2, so set the filter that way.
251 */
252 set_coefficient(bq, A * A, 0, 0, 1, 0, 0);
253 }
254 } else {
255 /* When frequency is 0 or 1, the z-transform is 1. */
256 set_coefficient(bq, 1, 0, 0, 1, 0, 0);
257 }
258 }
259
biquad_notch(struct biquad * bq,double frequency,double Q)260 static void biquad_notch(struct biquad *bq, double frequency, double Q)
261 {
262 /* Clip frequencies to between 0 and 1, inclusive. */
263 frequency = max(0.0, min(frequency, 1.0));
264
265 /* Don't let Q go negative, which causes an unstable filter. */
266 Q = max(0.0, Q);
267
268 if (frequency > 0 && frequency < 1) {
269 if (Q > 0) {
270 double w0 = M_PI * frequency;
271 double alpha = sin(w0) / (2 * Q);
272 double k = cos(w0);
273
274 double b0 = 1;
275 double b1 = -2 * k;
276 double b2 = 1;
277 double a0 = 1 + alpha;
278 double a1 = -2 * k;
279 double a2 = 1 - alpha;
280
281 set_coefficient(bq, b0, b1, b2, a0, a1, a2);
282 } else {
283 /* When Q = 0, the above formulas have problems. If we
284 * look at the z-transform, we can see that the limit
285 * as Q->0 is 0, so set the filter that way.
286 */
287 set_coefficient(bq, 0, 0, 0, 1, 0, 0);
288 }
289 } else {
290 /* When frequency is 0 or 1, the z-transform is 1. */
291 set_coefficient(bq, 1, 0, 0, 1, 0, 0);
292 }
293 }
294
biquad_allpass(struct biquad * bq,double frequency,double Q)295 static void biquad_allpass(struct biquad *bq, double frequency, double Q)
296 {
297 /* Clip frequencies to between 0 and 1, inclusive. */
298 frequency = max(0.0, min(frequency, 1.0));
299
300 /* Don't let Q go negative, which causes an unstable filter. */
301 Q = max(0.0, Q);
302
303 if (frequency > 0 && frequency < 1) {
304 if (Q > 0) {
305 double w0 = M_PI * frequency;
306 double alpha = sin(w0) / (2 * Q);
307 double k = cos(w0);
308
309 double b0 = 1 - alpha;
310 double b1 = -2 * k;
311 double b2 = 1 + alpha;
312 double a0 = 1 + alpha;
313 double a1 = -2 * k;
314 double a2 = 1 - alpha;
315
316 set_coefficient(bq, b0, b1, b2, a0, a1, a2);
317 } else {
318 /* When Q = 0, the above formulas have problems. If we
319 * look at the z-transform, we can see that the limit
320 * as Q->0 is -1, so set the filter that way.
321 */
322 set_coefficient(bq, -1, 0, 0, 1, 0, 0);
323 }
324 } else {
325 /* When frequency is 0 or 1, the z-transform is 1. */
326 set_coefficient(bq, 1, 0, 0, 1, 0, 0);
327 }
328 }
329
biquad_set(struct biquad * bq,enum biquad_type type,double freq,double Q,double gain)330 void biquad_set(struct biquad *bq, enum biquad_type type, double freq, double Q,
331 double gain)
332 {
333 /* Default is an identity filter. Also clear history values. */
334 set_coefficient(bq, 1, 0, 0, 1, 0, 0);
335 bq->x1 = 0;
336 bq->x2 = 0;
337 bq->y1 = 0;
338 bq->y2 = 0;
339
340 switch (type) {
341 case BQ_LOWPASS:
342 biquad_lowpass(bq, freq, Q);
343 break;
344 case BQ_HIGHPASS:
345 biquad_highpass(bq, freq, Q);
346 break;
347 case BQ_BANDPASS:
348 biquad_bandpass(bq, freq, Q);
349 break;
350 case BQ_LOWSHELF:
351 biquad_lowshelf(bq, freq, gain);
352 break;
353 case BQ_HIGHSHELF:
354 biquad_highshelf(bq, freq, gain);
355 break;
356 case BQ_PEAKING:
357 biquad_peaking(bq, freq, Q, gain);
358 break;
359 case BQ_NOTCH:
360 biquad_notch(bq, freq, Q);
361 break;
362 case BQ_ALLPASS:
363 biquad_allpass(bq, freq, Q);
364 break;
365 case BQ_NONE:
366 break;
367 }
368 }
369