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