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