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 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 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 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 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 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 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 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 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 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 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
