1 /** 2 * Basic IIR 1-pole and 2-pole filters through biquads. 3 * 4 * Copyright: Copyright Auburn Sounds 2015-2017. 5 * License: $(LINK2 http://www.boost.org/LICENSE_1_0.txt, Boost License 1.0) 6 * Authors: Guillaume Piolat 7 */ 8 module dplug.dsp.iir; 9 10 import std.math; 11 12 // DMD with a 32-bit target uses the FPU 13 version(X86) 14 { 15 version(DigitalMars) 16 { 17 version = killDenormals; 18 } 19 } 20 21 // TODO: function to make biquads from z-plane poles and zeroes 22 23 public 24 { 25 26 /// Type which hold the biquad coefficients. 27 /// Important: Coefficients are considered always normalized by a0. 28 /// Note: coeff[0] is b0, 29 /// coeff[1] is b1, 30 /// coeff[2] is b2, 31 /// coeff[3] is a1, 32 /// coeff[4] is a2 in the litterature. 33 alias BiquadCoeff = float[5]; 34 35 /// Maintain state for a biquad state. 36 /// To use an IIR filter you need an IIRDelay + one IIRCoeff. 37 struct BiquadDelay 38 { 39 enum order = 2; 40 41 double _x0; 42 double _x1; 43 double _y0; 44 double _y1; 45 46 void initialize() nothrow @nogc 47 { 48 _x0 = 0; 49 _x1 = 0; 50 _y0 = 0; 51 _y1 = 0; 52 } 53 54 static if (order == 2) 55 { 56 float nextSample(float input, const(BiquadCoeff) coeff) nothrow @nogc 57 { 58 double x1 = _x0, 59 x2 = _x1, 60 y1 = _y0, 61 y2 = _y1; 62 63 double a0 = coeff[0], 64 a1 = coeff[1], 65 a2 = coeff[2], 66 a3 = coeff[3], 67 a4 = coeff[4]; 68 69 double current = a0 * input + a1 * x1 + a2 * x2 - a3 * y1 - a4 * y2; 70 71 // kill denormals,and double values that would be converted 72 // to float denormals 73 version(killDenormals) 74 { 75 current += 1e-18f; 76 current -= 1e-18f; 77 } 78 79 _x0 = input; 80 _x1 = x1; 81 _y0 = current; 82 _y1 = y1; 83 return current; 84 } 85 86 void nextBuffer(const(float)* input, float* output, int frames, const(BiquadCoeff) coeff) nothrow @nogc 87 { 88 static if (D_InlineAsm_Any) 89 { 90 double x0 = _x0, 91 x1 = _x1, 92 y0 = _y0, 93 y1 = _y1; 94 95 double a0 = coeff[0], 96 a1 = coeff[1], 97 a2 = coeff[2], 98 a3 = coeff[3], 99 a4 = coeff[4]; 100 101 version(D_InlineAsm_X86) 102 { 103 asm nothrow @nogc 104 { 105 mov EAX, input; 106 mov EDX, output; 107 mov ECX, frames; 108 109 movlpd XMM0, qword ptr x0; // XMM0 = x1 x0 110 movhpd XMM0, qword ptr x1; 111 movlpd XMM1, qword ptr y0; // XMM1 = y1 y0 112 movhpd XMM1, qword ptr y1; 113 114 movlpd XMM2, qword ptr a1; // XMM2 = a2 a1 115 movhpd XMM2, qword ptr a2; 116 movlpd XMM3, qword ptr a3; // XMM3 = a4 a3 117 movhpd XMM3, qword ptr a4; 118 119 movq XMM4, qword ptr a0; // XMM4 = 0 a0 120 121 align 16; 122 loop: 123 pxor XMM5, XMM5; 124 cvtss2sd XMM5, dword ptr [EAX]; 125 126 movapd XMM6, XMM0; 127 movapd XMM7, XMM1; 128 129 mulpd XMM5, XMM4; // input[i]*a0 130 mulpd XMM6, XMM2; // x1*a2 x0*a1 131 mulpd XMM7, XMM3; // y1*a4 y0*a3 132 133 addpd XMM5, XMM6; 134 subpd XMM5, XMM7; // x1*a2 - y1*a4 | input[i]*a0 + x0*a1 - y0*a3 135 136 movapd XMM6, XMM5; 137 pslldq XMM0, 8; 138 psrldq XMM6, 8; 139 140 cvtss2sd XMM0, dword ptr [EAX]; // XMM0 <- x0 input[i] 141 addpd XMM5, XMM6; // garbage | input[i]*a0 + x0*a1 - y0*a3 + x1*a2 - y1*a4 142 143 cvtsd2ss XMM7, XMM5; 144 punpcklqdq XMM5, XMM1; // XMM5 <- y0 current 145 add EAX, 4; 146 movd dword ptr [EDX], XMM7; 147 add EDX, 4; 148 movapd XMM1, XMM5; 149 150 sub ECX, 1; 151 jnz short loop; 152 153 movlpd qword ptr x0, XMM0; 154 movhpd qword ptr x1, XMM0; 155 movlpd qword ptr y0, XMM1; 156 movhpd qword ptr y1, XMM1; 157 } 158 } 159 else version(D_InlineAsm_X86_64) 160 { 161 ubyte[16] storage0; 162 ubyte[16] storage1; 163 asm nothrow @nogc 164 { 165 movups storage0, XMM6; 166 movups storage1, XMM7; 167 168 mov RAX, input; 169 mov RDX, output; 170 mov ECX, frames; 171 172 movlpd XMM0, qword ptr x0; // XMM0 = x1 x0 173 movhpd XMM0, qword ptr x1; 174 movlpd XMM1, qword ptr y0; // XMM1 = y1 y0 175 movhpd XMM1, qword ptr y1; 176 177 movlpd XMM2, qword ptr a1; // XMM2 = a2 a1 178 movhpd XMM2, qword ptr a2; 179 movlpd XMM3, qword ptr a3; // XMM3 = a4 a3 180 movhpd XMM3, qword ptr a4; 181 182 movq XMM4, qword ptr a0; // XMM4 = 0 a0 183 184 align 16; 185 loop: 186 pxor XMM5, XMM5; 187 cvtss2sd XMM5, dword ptr [RAX]; 188 189 movapd XMM6, XMM0; 190 movapd XMM7, XMM1; 191 192 mulpd XMM5, XMM4; // input[i]*a0 193 mulpd XMM6, XMM2; // x1*a2 x0*a1 194 mulpd XMM7, XMM3; // y1*a4 y0*a3 195 196 addpd XMM5, XMM6; 197 subpd XMM5, XMM7; // x1*a2 - y1*a4 | input[i]*a0 + x0*a1 - y0*a3 198 199 movapd XMM6, XMM5; 200 pslldq XMM0, 8; 201 psrldq XMM6, 8; 202 203 addpd XMM5, XMM6; // garbage | input[i]*a0 + x0*a1 - y0*a3 + x1*a2 - y1*a4 204 cvtss2sd XMM0, dword ptr [RAX]; // XMM0 <- x0 input[i] 205 cvtsd2ss XMM7, XMM5; 206 punpcklqdq XMM5, XMM1; // XMM5 <- y0 current 207 add RAX, 4; 208 movd dword ptr [RDX], XMM7; 209 add RDX, 4; 210 movapd XMM1, XMM5; 211 212 sub ECX, 1; 213 jnz short loop; 214 215 movlpd qword ptr x0, XMM0; 216 movhpd qword ptr x1, XMM0; 217 movlpd qword ptr y0, XMM1; 218 movhpd qword ptr y1, XMM1; 219 220 movups XMM6, storage0; // XMMx with x >= 6 registers need to be preserved 221 movups XMM7, storage1; 222 } 223 } 224 else 225 static assert(false, "Not implemented for this platform."); 226 227 // Kill small signals that can cause denormals (no precision loss was measurable) 228 version(killDenormals) 229 { 230 x0 += 1e-10; 231 x0 -= 1e-10; 232 x1 += 1e-10; 233 x1 -= 1e-10; 234 y0 += 1e-10; 235 y0 -= 1e-10; 236 y1 += 1e-10; 237 y1 -= 1e-10; 238 } 239 240 _x0 = x0; 241 _x1 = x1; 242 _y0 = y0; 243 _y1 = y1; 244 } 245 else 246 { 247 double x0 = _x0, 248 x1 = _x1, 249 y0 = _y0, 250 y1 = _y1; 251 252 double a0 = coeff[0], 253 a1 = coeff[1], 254 a2 = coeff[2], 255 a3 = coeff[3], 256 a4 = coeff[4]; 257 258 for(int i = 0; i < frames; ++i) 259 { 260 double current = a0 * input[i] + a1 * x0 + a2 * x1 - a3 * y0 - a4 * y1; 261 262 // kill denormals,and double values that would be converted 263 // to float denormals 264 version(killDenormals) 265 { 266 current += 1e-18f; 267 current -= 1e-18f; 268 } 269 270 x1 = x0; 271 x0 = input[i]; 272 y1 = y0; 273 y0 = current; 274 output[i] = current; 275 } 276 277 _x0 = x0; 278 _x1 = x1; 279 _y0 = y0; 280 _y1 = y1; 281 } 282 } 283 } 284 } 285 286 deprecated("This function was renamed to biquadOnePoleLowPassImprecise.") 287 alias lowpassFilter1Pole = biquadOnePoleLowPassImprecise; 288 /// 1-pole low-pass filter, frequency mapping is not precise. 289 /// Not accurate across sample rates, but coefficient computation is cheap. Not advised. 290 BiquadCoeff biquadOnePoleLowPassImprecise(double frequency, double samplerate) nothrow @nogc 291 { 292 double t0 = frequency / samplerate; 293 if (t0 > 0.5f) 294 t0 = 0.5f; 295 296 double t1 = (1 - 2 * t0); 297 double t2 = t1 * t1; 298 299 BiquadCoeff result; 300 result[0] = cast(float)(1 - t2); 301 result[1] = 0; 302 result[2] = 0; 303 result[3] = cast(float)(-t2); 304 result[4] = 0; 305 return result; 306 } 307 308 deprecated("This function was renamed to biquadOnePoleHighPassImprecise.") 309 alias highpassFilter1Pole = biquadOnePoleHighPassImprecise; 310 /// 1-pole high-pass filter, frequency mapping is not precise. 311 /// Not accurate across sample rates, but coefficient computation is cheap. Not advised. 312 BiquadCoeff biquadOnePoleHighPassImprecise(double frequency, double samplerate) nothrow @nogc 313 { 314 double t0 = frequency / samplerate; 315 if (t0 > 0.5f) 316 t0 = 0.5f; 317 318 double t1 = (2 * t0); 319 double t2 = t1 * t1; 320 321 BiquadCoeff result; 322 result[0] = cast(float)(1 - t2); 323 result[1] = 0; 324 result[2] = 0; 325 result[3] = cast(float)(t2); 326 result[4] = 0; 327 return result; 328 } 329 330 /// Allpass interpolator. 331 /// https://ccrma.stanford.edu/~jos/pasp/First_Order_Allpass_Interpolation.html 332 /// http://users.spa.aalto.fi/vpv/publications/vesan_vaitos/ch3_pt3_allpass.pdf 333 /// It is recommended to use the range [0.5 .. 1.5] for best phase results. 334 /// Also known as Thiran filter. 335 deprecated BiquadCoeff allpassThiran1stOrder(T)(double fractionalDelay) nothrow @nogc 336 { 337 assert(fractionalDelay >= 0); 338 double eta = (1 - fractionalDelay) / (1 + fractionalDelay); 339 340 BiquadCoeff result; 341 result[0] = cast(T)(eta); 342 result[1] = 1; 343 result[2] = 0; 344 result[3] = cast(T)(eta); 345 result[4] = 0; 346 return result; 347 } 348 349 350 /// Same but 2nd order. 351 /// http://users.spa.aalto.fi/vpv/publications/vesan_vaitos/ch3_pt3_allpass.pdf 352 deprecated BiquadCoeff allpassThiran2ndOrder(T)(double fractionalDelay) nothrow @nogc 353 { 354 assert(fractionalDelay >= 0); 355 double a1 = 2 * (2 - fractionalDelay) / (1 + fractionalDelay); 356 double a2 = (fractionalDelay - 1) * (fractionalDelay - 2) 357 / 358 (fractionalDelay + 1) * (fractionalDelay + 2); 359 360 BiquadCoeff result; 361 result[0] = cast(T)(a1); 362 result[1] = 1; 363 result[2] = cast(T)(a2); 364 result[3] = cast(T)(a1); 365 result[4] = cast(T)(a2); 366 return result; 367 } 368 369 // Cookbook formulae for audio EQ biquad filter coefficients 370 // by Robert Bristow-Johnson 371 372 deprecated("This function was renamed to biquadRBJLowPass.") 373 alias lowpassFilterRBJ = biquadRBJLowPass; 374 /// Low-pass filter 12 dB/oct as described by Robert Bristow-Johnson. 375 BiquadCoeff biquadRBJLowPass(double frequency, double samplerate, double Q = SQRT1_2) nothrow @nogc 376 { 377 return generateBiquad(BiquadType.LOW_PASS_FILTER, frequency, samplerate, 0, Q); 378 } 379 380 deprecated("This function was renamed to biquadRBJHighPass.") 381 alias highpassFilterRBJ = biquadRBJHighPass; 382 /// High-pass filter 12 dB/oct as described by Robert Bristow-Johnson. 383 BiquadCoeff biquadRBJHighPass(double frequency, double samplerate, double Q = SQRT1_2) nothrow @nogc 384 { 385 return generateBiquad(BiquadType.HIGH_PASS_FILTER, frequency, samplerate, 0, Q); 386 } 387 388 deprecated("This function was renamed to biquadRBJBandPass.") 389 alias bandpassFilterRBJ = biquadRBJBandPass; 390 /// Band-pass filter as described by Robert Bristow-Johnson. 391 BiquadCoeff biquadRBJBandPass(double frequency, double samplerate, double Q = SQRT1_2) nothrow @nogc 392 { 393 return generateBiquad(BiquadType.BAND_PASS_FILTER, frequency, samplerate, 0, Q); 394 } 395 396 deprecated("This function was renamed to biquadRBJNotch.") 397 alias notchFilterRBJ = biquadRBJNotch; 398 /// Notch filter as described by Robert Bristow-Johnson. 399 BiquadCoeff biquadRBJNotch(double frequency, double samplerate, double Q = SQRT1_2) nothrow @nogc 400 { 401 return generateBiquad(BiquadType.NOTCH_FILTER, frequency, samplerate, 0, Q); 402 } 403 404 deprecated("This function was renamed to biquadRBJPeak.") 405 alias peakFilterRBJ = biquadRBJPeak; 406 /// Peak filter as described by Robert Bristow-Johnson. 407 BiquadCoeff biquadRBJPeak(double frequency, double samplerate, double gain, double Q = SQRT1_2) nothrow @nogc 408 { 409 return generateBiquad(BiquadType.PEAK_FILTER, frequency, samplerate, gain, Q); 410 } 411 412 deprecated("This function was renamed to biquadRBJLowShelf.") 413 alias lowShelfFilterRBJ = biquadRBJLowShelf; 414 /// Low-shelf filter as described by Robert Bristow-Johnson. 415 BiquadCoeff biquadRBJLowShelf(double frequency, double samplerate, double gain, double Q = SQRT1_2) nothrow @nogc 416 { 417 return generateBiquad(BiquadType.LOW_SHELF, frequency, samplerate, gain, Q); 418 } 419 420 deprecated("This function was renamed to biquadRBJHighShelf.") 421 alias highShelfFilterRBJ = biquadRBJHighShelf; 422 /// High-shelf filter as described by Robert Bristow-Johnson. 423 BiquadCoeff biquadRBJHighShelf(double frequency, double samplerate, double gain, double Q = SQRT1_2) nothrow @nogc 424 { 425 return generateBiquad(BiquadType.HIGH_SHELF, frequency, samplerate, gain, Q); 426 } 427 428 deprecated("This function was renamed to biquadBypass.") 429 alias bypassFilter = biquadBypass; 430 /// Identity biquad, pass signal unchanged. 431 BiquadCoeff biquadBypass() nothrow @nogc 432 { 433 BiquadCoeff result; 434 result[0] = 1; 435 result[1] = 0; 436 result[2] = 0; 437 result[3] = 0; 438 result[4] = 0; 439 return result; 440 } 441 442 deprecated("This function was renamed to biquadZero.") 443 alias zeroFilter = biquadZero; 444 /// Zero biquad, gives zero output. 445 BiquadCoeff biquadZero() nothrow @nogc 446 { 447 BiquadCoeff result; 448 result[0] = 0; 449 result[1] = 0; 450 result[2] = 0; 451 result[3] = 0; 452 result[4] = 0; 453 return result; 454 } 455 } 456 457 private 458 { 459 enum BiquadType 460 { 461 LOW_PASS_FILTER, 462 HIGH_PASS_FILTER, 463 BAND_PASS_FILTER, 464 NOTCH_FILTER, 465 PEAK_FILTER, 466 LOW_SHELF, 467 HIGH_SHELF 468 } 469 470 // generates RBJ biquad coefficients 471 BiquadCoeff generateBiquad(BiquadType type, double frequency, double samplerate, double gaindB, double Q) nothrow @nogc 472 { 473 // regardless of the output precision, always compute coefficients in double precision 474 475 double A = pow(10.0, gaindB / 40.0); 476 double w0 = (2.0 * PI) * frequency / samplerate; 477 double sin_w0 = sin(w0); 478 double cos_w0 = cos(w0); 479 480 double alpha = sin_w0 / (2 * Q); 481 482 // = sin(w0)*sinh( ln(2)/2 * BW * w0/sin(w0) ) (case: BW) 483 // = sin(w0)/2 * sqrt( (A + 1/A)*(1/S - 1) + 2 ) (case: S) 484 485 double b0, b1, b2, a0, a1, a2; 486 487 final switch(type) 488 { 489 case BiquadType.LOW_PASS_FILTER: 490 b0 = (1 - cos_w0) / 2; 491 b1 = 1 - cos_w0; 492 b2 = (1 - cos_w0) / 2; 493 a0 = 1 + alpha; 494 a1 = -2 * cos_w0; 495 a2 = 1 - alpha; 496 break; 497 498 case BiquadType.HIGH_PASS_FILTER: 499 b0 = (1 + cos_w0) / 2; 500 b1 = -(1 + cos_w0); 501 b2 = (1 + cos_w0) / 2; 502 a0 = 1 + alpha; 503 a1 = -2 * cos_w0; 504 a2 = 1 - alpha; 505 break; 506 507 case BiquadType.BAND_PASS_FILTER: 508 b0 = alpha; 509 b1 = 0; 510 b2 = -alpha; 511 a0 = 1 + alpha; 512 a1 = -2 * cos_w0; 513 a2 = 1 - alpha; 514 break; 515 516 case BiquadType.NOTCH_FILTER: 517 b0 = 1; 518 b1 = -2*cos_w0; 519 b2 = 1; 520 a0 = 1 + alpha; 521 a1 = -2*cos(w0); 522 a2 = 1 - alpha; 523 break; 524 525 case BiquadType.PEAK_FILTER: 526 b0 = 1 + alpha * A; 527 b1 = -2 * cos_w0; 528 b2 = 1 - alpha * A; 529 a0 = 1 + alpha / A; 530 a1 = -2 * cos_w0; 531 a2 = 1 - alpha / A; 532 break; 533 534 case BiquadType.LOW_SHELF: 535 { 536 double ap1 = A + 1; 537 double am1 = A - 1; 538 double M = 2 * sqrt(A) * alpha; 539 b0 = A * (ap1 - am1 * cos_w0 + M); 540 b1 = 2 * A * (am1 - ap1 * cos_w0); 541 b2 = A * (ap1 - am1 * cos_w0 - M); 542 a0 = ap1 + am1 * cos_w0 + M; 543 a1 = -2 * (am1 + ap1 * cos_w0); 544 a2 = ap1 + am1 * cos_w0 - M; 545 } 546 break; 547 548 case BiquadType.HIGH_SHELF: 549 { 550 double ap1 = A + 1; 551 double am1 = A - 1; 552 double M = 2 * sqrt(A) * alpha; 553 b0 = A * (ap1 + am1 * cos_w0 + M); 554 b1 = -2 * A * (am1 + ap1 * cos_w0); 555 b2 = A * (ap1 + am1 * cos_w0 - M); 556 a0 = ap1 - am1 * cos_w0 + M; 557 a1 = 2 * (am1 - ap1 * cos_w0); 558 a2 = ap1 - am1 * cos_w0 - M; 559 } 560 break; 561 } 562 563 BiquadCoeff result; 564 result[0] = cast(float)(b0 / a0); 565 result[1] = cast(float)(b1 / a0); 566 result[2] = cast(float)(b2 / a0); 567 result[3] = cast(float)(a1 / a0); 568 result[4] = cast(float)(a2 / a0); 569 return result; 570 } 571 } 572 573 574 version(D_InlineAsm_X86) 575 private enum D_InlineAsm_Any = true; 576 else version(D_InlineAsm_X86_64) 577 private enum D_InlineAsm_Any = true; 578 else 579 private enum D_InlineAsm_Any = false;