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