• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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