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
12 #include "config.h"
13
14 #include <spa/utils/defs.h>
15
16 #include <math.h>
17 #include "biquad.h"
18
19 #ifndef M_PI
20 #define M_PI 3.14159265358979323846
21 #endif
22
23 #ifndef M_SQRT2
24 #define M_SQRT2 1.41421356237309504880
25 #endif
26
set_coefficient(struct biquad * bq,double b0,double b1,double b2,double a0,double a1,double a2)27 static void set_coefficient(struct biquad *bq, double b0, double b1, double b2,
28 double a0, double a1, double a2)
29 {
30 double a0_inv = 1 / a0;
31 bq->b0 = b0 * a0_inv;
32 bq->b1 = b1 * a0_inv;
33 bq->b2 = b2 * a0_inv;
34 bq->a1 = a1 * a0_inv;
35 bq->a2 = a2 * a0_inv;
36 }
37
biquad_lowpass(struct biquad * bq,double cutoff)38 static void biquad_lowpass(struct biquad *bq, double cutoff)
39 {
40 /* Limit cutoff to 0 to 1. */
41 cutoff = SPA_CLAMP(cutoff, 0.0, 1.0);
42
43 if (cutoff >= 1.0) {
44 /* When cutoff is 1, the z-transform is 1. */
45 set_coefficient(bq, 1, 0, 0, 1, 0, 0);
46 } else if (cutoff > 0) {
47 /* Compute biquad coefficients for lowpass filter */
48 double theta = M_PI * cutoff;
49 double sn = 0.5 * M_SQRT2 * sin(theta);
50 double beta = 0.5 * (1 - sn) / (1 + sn);
51 double gamma_coeff = (0.5 + beta) * cos(theta);
52 double alpha = 0.25 * (0.5 + beta - gamma_coeff);
53
54 double b0 = 2 * alpha;
55 double b1 = 2 * 2 * alpha;
56 double b2 = 2 * alpha;
57 double a1 = 2 * -gamma_coeff;
58 double a2 = 2 * beta;
59
60 set_coefficient(bq, b0, b1, b2, 1, a1, a2);
61 } else {
62 /* When cutoff is zero, nothing gets through the filter, so set
63 * coefficients up correctly.
64 */
65 set_coefficient(bq, 0, 0, 0, 1, 0, 0);
66 }
67 }
68
biquad_highpass(struct biquad * bq,double cutoff)69 static void biquad_highpass(struct biquad *bq, double cutoff)
70 {
71 /* Limit cutoff to 0 to 1. */
72 cutoff = SPA_CLAMP(cutoff, 0.0, 1.0);
73
74 if (cutoff >= 1.0) {
75 /* The z-transform is 0. */
76 set_coefficient(bq, 0, 0, 0, 1, 0, 0);
77 } else if (cutoff > 0) {
78 /* Compute biquad coefficients for highpass filter */
79 double theta = M_PI * cutoff;
80 double sn = 0.5 * M_SQRT2 * sin(theta);
81 double beta = 0.5 * (1 - sn) / (1 + sn);
82 double gamma_coeff = (0.5 + beta) * cos(theta);
83 double alpha = 0.25 * (0.5 + beta + gamma_coeff);
84
85 double b0 = 2 * alpha;
86 double b1 = 2 * -2 * alpha;
87 double b2 = 2 * alpha;
88 double a1 = 2 * -gamma_coeff;
89 double a2 = 2 * beta;
90
91 set_coefficient(bq, b0, b1, b2, 1, a1, a2);
92 } else {
93 /* When cutoff is zero, we need to be careful because the above
94 * gives a quadratic divided by the same quadratic, with poles
95 * and zeros on the unit circle in the same place. When cutoff
96 * is zero, the z-transform is 1.
97 */
98 set_coefficient(bq, 1, 0, 0, 1, 0, 0);
99 }
100 }
101
biquad_set(struct biquad * bq,enum biquad_type type,double freq)102 void biquad_set(struct biquad *bq, enum biquad_type type, double freq)
103 {
104
105 switch (type) {
106 case BQ_LOWPASS:
107 biquad_lowpass(bq, freq);
108 break;
109 case BQ_HIGHPASS:
110 biquad_highpass(bq, freq);
111 break;
112 }
113 }
114