libs/ardour/dsp_filter.cc

   1 /*
   2  * Copyright (C) 2016 Robin Gareus <robin@gareus.org>
   3  *
   4  * This program is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU General Public License
   6  * as published by the Free Software Foundation; either version 2
   7  * of the License, or (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program; if not, write to the Free Software
  16  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  17  *
  18  */
  19
  20 #include <algorithm>
  21 #include <stdlib.h>
  22 #include <cmath>
  23 #include "ardour/dB.h"
  24 #include "ardour/buffer.h"
  25 #include "ardour/dsp_filter.h"
  26
  27 #ifdef COMPILER_MSVC
  28 #include <float.h>
  29 #define isfinite_local(val) (bool)_finite((double)val)
  30 #else
  31 #define isfinite_local std::isfinite
  32 #endif
  33
  34 #ifndef M_PI
  35 #define M_PI 3.14159265358979323846
  36 #endif
  37
  38 using namespace ARDOUR::DSP;
  39
  40 void
  41 ARDOUR::DSP::memset (float *data, const float val, const uint32_t n_samples) {
  42         for (uint32_t i = 0; i < n_samples; ++i) {
  43                 data[i] = val;
  44         }
  45 }
  46
  47 void
  48 ARDOUR::DSP::mmult (float *data, float *mult, const uint32_t n_samples) {
  49         for (uint32_t i = 0; i < n_samples; ++i) {
  50                 data[i] *= mult[i];
  51         }
  52 }
  53
  54 float
  55 ARDOUR::DSP::log_meter (float power) {
  56         // compare to libs/ardour/log_meter.h
  57         static const float lower_db = -192.f;
  58         static const float upper_db = 0.f;
  59         static const float non_linearity = 8.0;
  60         return (power < lower_db ? 0.0 : powf ((power - lower_db) / (upper_db - lower_db), non_linearity));
  61 }
  62
  63 float
  64 ARDOUR::DSP::log_meter_coeff (float coeff) {
  65         if (coeff <= 0) return 0;
  66         return log_meter (fast_coefficient_to_dB (coeff));
  67 }
  68
  69 void
  70 ARDOUR::DSP::peaks (const float *data, float &min, float &max, uint32_t n_samples) {
  71         for (uint32_t i = 0; i < n_samples; ++i) {
  72                 if (data[i] < min) min = data[i];
  73                 if (data[i] > max) max = data[i];
  74         }
  75 }
  76
  77 void
  78 ARDOUR::DSP::process_map (BufferSet* bufs, const ChanMapping& in, const ChanMapping& out, pframes_t nframes, samplecnt_t offset, const DataType& dt)
  79 {
  80         const ChanMapping::Mappings& im (in.mappings());
  81
  82         for (ChanMapping::Mappings::const_iterator tm = im.begin(); tm != im.end(); ++tm) {
  83                 if (tm->first != dt) { continue; }
  84                 for (ChanMapping::TypeMapping::const_iterator i = tm->second.begin(); i != tm->second.end(); ++i) {
  85                         bool valid;
  86                         const uint32_t idx = out.get (dt, i->second, &valid);
  87                         if (valid && idx != i->first) {
  88                                 bufs->get (dt, idx).read_from (bufs->get (dt, i->first), nframes, offset, offset);
  89                         }
  90                 }
  91         }
  92         for (ChanMapping::Mappings::const_iterator tm = im.begin(); tm != im.end(); ++tm) {
  93                 if (tm->first != dt) { continue; }
  94                 for (ChanMapping::TypeMapping::const_iterator i = tm->second.begin(); i != tm->second.end(); ++i) {
  95                         bool valid;
  96                         in.get_src (dt, i->first, &valid);
  97                         if (!valid) {
  98                                 bufs->get (dt, i->second).silence (nframes, offset);
  99                         }
 100                 }
 101         }
 102
 103 }
 104
 105 LowPass::LowPass (double samplerate, float freq)
 106         : _rate (samplerate)
 107         , _z (0)
 108 {
 109         set_cutoff (freq);
 110 }
 111
 112 void
 113 LowPass::set_cutoff (float freq)
 114 {
 115         _a = 1.f - expf (-2.f * M_PI * freq / _rate);
 116 }
 117
 118 void
 119 LowPass::proc (float *data, const uint32_t n_samples)
 120 {
 121         // localize variables
 122         const float a = _a;
 123         float z = _z;
 124         for (uint32_t i = 0; i < n_samples; ++i) {
 125                 data[i] += a * (data[i] - z);
 126                 z = data[i];
 127         }
 128         _z = z;
 129         if (!isfinite_local (_z)) { _z = 0; }
 130 }
 131
 132 void
 133 LowPass::ctrl (float *data, const float val, const uint32_t n_samples)
 134 {
 135         // localize variables
 136         const float a = _a;
 137         float z = _z;
 138         for (uint32_t i = 0; i < n_samples; ++i) {
 139                 data[i] += a * (val - z);
 140                 z = data[i];
 141         }
 142         _z = z;
 143 }
 144
 145 ///////////////////////////////////////////////////////////////////////////////
 146
 147 Biquad::Biquad (double samplerate)
 148         : _rate (samplerate)
 149         , _z1 (0.0)
 150         , _z2 (0.0)
 151         , _a1 (0.0)
 152         , _a2 (0.0)
 153         , _b0 (1.0)
 154         , _b1 (0.0)
 155         , _b2 (0.0)
 156 {
 157 }
 158
 159 Biquad::Biquad (const Biquad &other)
 160         : _rate (other._rate)
 161         , _z1 (0.0)
 162         , _z2 (0.0)
 163         , _a1 (other._a1)
 164         , _a2 (other._a2)
 165         , _b0 (other._b0)
 166         , _b1 (other._b1)
 167         , _b2 (other._b2)
 168 {
 169 }
 170
 171 void
 172 Biquad::run (float *data, const uint32_t n_samples)
 173 {
 174         for (uint32_t i = 0; i < n_samples; ++i) {
 175                 const float xn = data[i];
 176                 const float z = _b0 * xn + _z1;
 177                 _z1           = _b1 * xn - _a1 * z + _z2;
 178                 _z2           = _b2 * xn - _a2 * z;
 179                 data[i] = z;
 180         }
 181
 182         if (!isfinite_local (_z1)) { _z1 = 0; }
 183         if (!isfinite_local (_z2)) { _z2 = 0; }
 184 }
 185
 186 void
 187 Biquad::configure (double a1, double a2, double b0, double b1, double b2)
 188 {
 189         _a1 = a1;
 190         _a2 = a2;
 191         _b0 = b0;
 192         _b1 = b1;
 193         _b2 = b2;
 194 }
 195
 196 void
 197 Biquad::compute (Type type, double freq, double Q, double gain)
 198 {
 199         if (Q <= .001)  { Q = 0.001; }
 200         if (freq <= 1.) { freq = 1.; }
 201         if (freq >= 0.4998 * _rate) { freq = 0.4998 * _rate; }
 202
 203         /* Compute biquad filter settings.
 204          * Based on 'Cookbook formulae for audio EQ biquad filter coefficents'
 205          * by Robert Bristow-Johnson
 206          */
 207         const double A = pow (10.0, (gain / 40.0));
 208         const double W0 = (2.0 * M_PI * freq) / _rate;
 209         const double sinW0 = sin (W0);
 210         const double cosW0 = cos (W0);
 211         const double alpha = sinW0 / (2.0 * Q);
 212         const double beta  = sqrt (A) / Q;
 213
 214         double _a0;
 215
 216         switch (type) {
 217                 case LowPass:
 218                         _b0 = (1.0 - cosW0) / 2.0;
 219                         _b1 =  1.0 - cosW0;
 220                         _b2 = (1.0 - cosW0) / 2.0;
 221                         _a0 =  1.0 + alpha;
 222                         _a1 = -2.0 * cosW0;
 223                         _a2 =  1.0 - alpha;
 224                         break;
 225
 226                 case HighPass:
 227                         _b0 =  (1.0 + cosW0) / 2.0;
 228                         _b1 = -(1.0 + cosW0);
 229                         _b2 =  (1.0 + cosW0) / 2.0;
 230                         _a0 =   1.0 + alpha;
 231                         _a1 =  -2.0 * cosW0;
 232                         _a2 =   1.0 - alpha;
 233                         break;
 234
 235                 case BandPassSkirt: /* Constant skirt gain, peak gain = Q */
 236                         _b0 =  sinW0 / 2.0;
 237                         _b1 =  0.0;
 238                         _b2 = -sinW0 / 2.0;
 239                         _a0 =  1.0 + alpha;
 240                         _a1 = -2.0 * cosW0;
 241                         _a2 =  1.0 - alpha;
 242                         break;
 243
 244                 case BandPass0dB: /* Constant 0 dB peak gain */
 245                         _b0 =  alpha;
 246                         _b1 =  0.0;
 247                         _b2 = -alpha;
 248                         _a0 =  1.0 + alpha;
 249                         _a1 = -2.0 * cosW0;
 250                         _a2 =  1.0 - alpha;
 251                         break;
 252
 253                 case Notch:
 254                         _b0 =  1.0;
 255                         _b1 = -2.0 * cosW0;
 256                         _b2 =  1.0;
 257                         _a0 =  1.0 + alpha;
 258                         _a1 = -2.0 * cosW0;
 259                         _a2 =  1.0 - alpha;
 260                         break;
 261
 262                 case AllPass:
 263                         _b0 =  1.0 - alpha;
 264                         _b1 = -2.0 * cosW0;
 265                         _b2 =  1.0 + alpha;
 266                         _a0 =  1.0 + alpha;
 267                         _a1 = -2.0 * cosW0;
 268                         _a2 =  1.0 - alpha;
 269                         break;
 270
 271                 case Peaking:
 272                         _b0 =  1.0 + (alpha * A);
 273                         _b1 = -2.0 * cosW0;
 274                         _b2 =  1.0 - (alpha * A);
 275                         _a0 =  1.0 + (alpha / A);
 276                         _a1 = -2.0 * cosW0;
 277                         _a2 =  1.0 - (alpha / A);
 278                         break;
 279
 280                 case LowShelf:
 281                         _b0 =         A * ((A + 1) - ((A - 1) * cosW0) + (beta * sinW0));
 282                         _b1 = (2.0 * A) * ((A - 1) - ((A + 1) * cosW0));
 283                         _b2 =         A * ((A + 1) - ((A - 1) * cosW0) - (beta * sinW0));
 284                         _a0 =              (A + 1) + ((A - 1) * cosW0) + (beta * sinW0);
 285                         _a1 =      -2.0 * ((A - 1) + ((A + 1) * cosW0));
 286                         _a2 =              (A + 1) + ((A - 1) * cosW0) - (beta * sinW0);
 287                         break;
 288
 289                 case HighShelf:
 290                         _b0 =          A * ((A + 1) + ((A - 1) * cosW0) + (beta * sinW0));
 291                         _b1 = -(2.0 * A) * ((A - 1) + ((A + 1) * cosW0));
 292                         _b2 =          A * ((A + 1) + ((A - 1) * cosW0) - (beta * sinW0));
 293                         _a0 =               (A + 1) - ((A - 1) * cosW0) + (beta * sinW0);
 294                         _a1 =        2.0 * ((A - 1) - ((A + 1) * cosW0));
 295                         _a2 =               (A + 1) - ((A - 1) * cosW0) - (beta * sinW0);
 296                         break;
 297                 default:
 298                         abort(); /*NOTREACHED*/
 299                         break;
 300         }
 301
 302         _b0 /= _a0;
 303         _b1 /= _a0;
 304         _b2 /= _a0;
 305         _a1 /= _a0;
 306         _a2 /= _a0;
 307 }
 308
 309 float
 310 Biquad::dB_at_freq (float freq) const
 311 {
 312         const double W0 = (2.0 * M_PI * freq) / _rate;
 313         const float c1 = cosf (W0);
 314         const float s1 = sinf (W0);
 315
 316         const float A = _b0 + _b2;
 317         const float B = _b0 - _b2;
 318         const float C = 1.0 + _a2;
 319         const float D = 1.0 - _a2;
 320
 321         const float a = A * c1 + _b1;
 322         const float b = B * s1;
 323         const float c = C * c1 + _a1;
 324         const float d = D * s1;
 325
 326 #define SQUARE(x) ( (x) * (x) )
 327         float rv = 20.f * log10f (sqrtf ((SQUARE(a) + SQUARE(b)) * (SQUARE(c) + SQUARE(d))) / (SQUARE(c) + SQUARE(d)));
 328         if (!isfinite_local (rv)) { rv = 0; }
 329         return std::min (120.f, std::max(-120.f, rv));
 330 }
 331
 332
 333 Glib::Threads::Mutex FFTSpectrum::fft_planner_lock;
 334
 335 FFTSpectrum::FFTSpectrum (uint32_t window_size, double rate)
 336         : hann_window (0)
 337 {
 338         init (window_size, rate);
 339 }
 340
 341 FFTSpectrum::~FFTSpectrum ()
 342 {
 343         {
 344                 Glib::Threads::Mutex::Lock lk (fft_planner_lock);
 345                 fftwf_destroy_plan (_fftplan);
 346         }
 347         fftwf_free (_fft_data_in);
 348         fftwf_free (_fft_data_out);
 349         free (_fft_power);
 350         free (hann_window);
 351 }
 352
 353 void
 354 FFTSpectrum::init (uint32_t window_size, double rate)
 355 {
 356         assert (window_size > 0);
 357         Glib::Threads::Mutex::Lock lk (fft_planner_lock);
 358
 359         _fft_window_size = window_size;
 360         _fft_data_size   = window_size / 2;
 361         _fft_freq_per_bin = rate / _fft_data_size / 2.f;
 362
 363         _fft_data_in  = (float *) fftwf_malloc (sizeof(float) * _fft_window_size);
 364         _fft_data_out = (float *) fftwf_malloc (sizeof(float) * _fft_window_size);
 365         _fft_power    = (float *) malloc (sizeof(float) * _fft_data_size);
 366
 367         reset ();
 368
 369         _fftplan = fftwf_plan_r2r_1d (_fft_window_size, _fft_data_in, _fft_data_out, FFTW_R2HC, FFTW_MEASURE);
 370
 371         hann_window  = (float *) malloc(sizeof(float) * window_size);
 372         double sum = 0.0;
 373
 374         for (uint32_t i = 0; i < window_size; ++i) {
 375                 hann_window[i] = 0.5f - (0.5f * (float) cos (2.0f * M_PI * (float)i / (float)(window_size)));
 376                 sum += hann_window[i];
 377         }
 378         const double isum = 2.0 / sum;
 379         for (uint32_t i = 0; i < window_size; ++i) {
 380                 hann_window[i] *= isum;
 381         }
 382 }
 383
 384 void
 385 FFTSpectrum::reset ()
 386 {
 387         for (uint32_t i = 0; i < _fft_data_size; ++i) {
 388                 _fft_power[i] = 0;
 389         }
 390         for (uint32_t i = 0; i < _fft_window_size; ++i) {
 391                 _fft_data_out[i] = 0;
 392         }
 393 }
 394
 395 void
 396 FFTSpectrum::set_data_hann (float const * const data, uint32_t n_samples, uint32_t offset)
 397 {
 398         assert(n_samples + offset <= _fft_window_size);
 399         for (uint32_t i = 0; i < n_samples; ++i) {
 400                 _fft_data_in[i + offset] = data[i] * hann_window[i + offset];
 401         }
 402 }
 403
 404 void
 405 FFTSpectrum::execute ()
 406 {
 407         fftwf_execute (_fftplan);
 408
 409         _fft_power[0] = _fft_data_out[0] * _fft_data_out[0];
 410
 411 #define FRe (_fft_data_out[i])
 412 #define FIm (_fft_data_out[_fft_window_size - i])
 413         for (uint32_t i = 1; i < _fft_data_size - 1; ++i) {
 414                 _fft_power[i] = (FRe * FRe) + (FIm * FIm);
 415                 //_fft_phase[i] = atan2f (FIm, FRe);
 416         }
 417 #undef FRe
 418 #undef FIm
 419 }
 420
 421 float
 422 FFTSpectrum::power_at_bin (const uint32_t b, const float norm) const {
 423         assert (b < _fft_data_size);
 424         const float a = _fft_power[b] * norm;
 425         return a > 1e-12 ? 10.0 * fast_log10 (a) : -INFINITY;
 426 }
 427
 428 Generator::Generator ()
 429         : _type (UniformWhiteNoise)
 430         , _rseed (1)
 431 {
 432         set_type (UniformWhiteNoise);
 433 }
 434
 435 void
 436 Generator::set_type (Generator::Type t) {
 437         _type = t;
 438         _b0 = _b1 = _b2 = _b3 = _b4 = _b5 = _b6 = 0;
 439         _pass = false;
 440         _rn = 0;
 441 }
 442
 443 void
 444 Generator::run (float *data, const uint32_t n_samples)
 445 {
 446         switch (_type) {
 447                 default:
 448                 case UniformWhiteNoise:
 449                         for (uint32_t i = 0; i < n_samples; ++i) {
 450                                 data[i] = randf();
 451                         }
 452                         break;
 453                 case GaussianWhiteNoise:
 454                         for (uint32_t i = 0 ; i < n_samples; ++i) {
 455                                 data[i] = 0.7079f * grandf();
 456                         }
 457                         break;
 458                 case PinkNoise:
 459                         for (uint32_t i = 0 ; i < n_samples; ++i) {
 460                                 const float white = .39572f * randf ();
 461                                 _b0 = .99886f * _b0 + white * .0555179f;
 462                                 _b1 = .99332f * _b1 + white * .0750759f;
 463                                 _b2 = .96900f * _b2 + white * .1538520f;
 464                                 _b3 = .86650f * _b3 + white * .3104856f;
 465                                 _b4 = .55000f * _b4 + white * .5329522f;
 466                                 _b5 = -.7616f * _b5 - white * .0168980f;
 467                                 data[i] = _b0 + _b1 + _b2 + _b3 + _b4 + _b5 + _b6 + white * 0.5362f;
 468                                 _b6 = white * 0.115926f;
 469                         }
 470                         break;
 471         }
 472 }
 473
 474 inline uint32_t
 475 Generator::randi ()
 476 {
 477         // 31bit Park-Miller-Carta Pseudo-Random Number Generator
 478         uint32_t hi, lo;
 479         lo = 16807 * (_rseed & 0xffff);
 480         hi = 16807 * (_rseed >> 16);
 481         lo += (hi & 0x7fff) << 16;
 482         lo += hi >> 15;
 483         lo = (lo & 0x7fffffff) + (lo >> 31);
 484         return (_rseed = lo);
 485 }
 486
 487 inline float
 488 Generator::grandf ()
 489 {
 490         float x1, x2, r;
 491
 492         if (_pass) {
 493                 _pass = false;
 494                 return _rn;
 495         }
 496
 497         do {
 498                 x1 = randf ();
 499                 x2 = randf ();
 500                 r = x1 * x1 + x2 * x2;
 501         } while ((r >= 1.0f) || (r < 1e-22f));
 502
 503         r = sqrtf (-2.f * logf (r) / r);
 504
 505         _pass = true;
 506         _rn = r * x2;
 507         return r * x1;
 508 }