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 gtk2_ardour/logmeter.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, framecnt_t offset, const DataType& dt)
  79 {
  80         const ChanMapping::Mappings& im (in.mappings());
  81         const ChanMapping::Mappings& om (out.mappings());
  82
  83         for (ChanMapping::Mappings::const_iterator tm = im.begin(); tm != im.end(); ++tm) {
  84                 if (tm->first != dt) { continue; }
  85                 for (ChanMapping::TypeMapping::const_iterator i = tm->second.begin(); i != tm->second.end(); ++i) {
  86                         bool valid;
  87                         const uint32_t idx = out.get (dt, i->second, &valid);
  88                         if (valid && idx != i->first) {
  89                                 bufs->get (dt, idx).read_from (bufs->get (dt, i->first), nframes, offset, offset);
  90                         }
  91                 }
  92         }
  93         for (ChanMapping::Mappings::const_iterator tm = im.begin(); tm != im.end(); ++tm) {
  94                 if (tm->first != dt) { continue; }
  95                 for (ChanMapping::TypeMapping::const_iterator i = tm->second.begin(); i != tm->second.end(); ++i) {
  96                         bool valid;
  97                         in.get_src (dt, i->first, &valid);
  98                         if (!valid) {
  99                                 bufs->get (dt, i->second).silence (nframes, offset);
 100                         }
 101                 }
 102         }
 103
 104 }
 105
 106 LowPass::LowPass (double samplerate, float freq)
 107         : _rate (samplerate)
 108         , _z (0)
 109 {
 110         set_cutoff (freq);
 111 }
 112
 113 void
 114 LowPass::set_cutoff (float freq)
 115 {
 116         _a = 1.f - expf (-2.f * M_PI * freq / _rate);
 117 }
 118
 119 void
 120 LowPass::proc (float *data, const uint32_t n_samples)
 121 {
 122         // localize variables
 123         const float a = _a;
 124         float z = _z;
 125         for (uint32_t i = 0; i < n_samples; ++i) {
 126                 data[i] += a * (data[i] - z);
 127                 z = data[i];
 128         }
 129         _z = z;
 130         if (!isfinite_local (_z)) { _z = 0; }
 131 }
 132
 133 void
 134 LowPass::ctrl (float *data, const float val, const uint32_t n_samples)
 135 {
 136         // localize variables
 137         const float a = _a;
 138         float z = _z;
 139         for (uint32_t i = 0; i < n_samples; ++i) {
 140                 data[i] += a * (val - z);
 141                 z = data[i];
 142         }
 143         _z = z;
 144 }
 145
 146 ///////////////////////////////////////////////////////////////////////////////
 147
 148 Biquad::Biquad (double samplerate)
 149         : _rate (samplerate)
 150         , _z1 (0.0)
 151         , _z2 (0.0)
 152         , _a1 (0.0)
 153         , _a2 (0.0)
 154         , _b0 (1.0)
 155         , _b1 (0.0)
 156         , _b2 (0.0)
 157 {
 158 }
 159
 160 Biquad::Biquad (const Biquad &other)
 161         : _rate (other._rate)
 162         , _z1 (0.0)
 163         , _z2 (0.0)
 164         , _a1 (other._a1)
 165         , _a2 (other._a2)
 166         , _b0 (other._b0)
 167         , _b1 (other._b1)
 168         , _b2 (other._b2)
 169 {
 170 }
 171
 172 void
 173 Biquad::run (float *data, const uint32_t n_samples)
 174 {
 175         for (uint32_t i = 0; i < n_samples; ++i) {
 176                 const float xn = data[i];
 177                 const float z = _b0 * xn + _z1;
 178                 _z1           = _b1 * xn - _a1 * z + _z2;
 179                 _z2           = _b2 * xn - _a2 * z;
 180                 data[i] = z;
 181         }
 182
 183         if (!isfinite_local (_z1)) { _z1 = 0; }
 184         if (!isfinite_local (_z2)) { _z2 = 0; }
 185 }
 186
 187 void
 188 Biquad::configure (double a1, double a2, double b0, double b1, double b2)
 189 {
 190         _a1 = a1;
 191         _a2 = a2;
 192         _b0 = b0;
 193         _b1 = b1;
 194         _b2 = b2;
 195 }
 196
 197 void
 198 Biquad::compute (Type type, double freq, double Q, double gain)
 199 {
 200         if (Q <= .001)     { Q = 0.001; }
 201         if (freq <= 1.)    { freq = 1.; }
 202         if (freq >= _rate) { freq = _rate; }
 203
 204         /* Compute biquad filter settings.
 205          * Based on 'Cookbook formulae for audio EQ biquad filter coefficents'
 206          * by Robert Bristow-Johnson
 207          */
 208         const double A = pow (10.0, (gain / 40.0));
 209         const double W0 = (2.0 * M_PI * freq) / _rate;
 210         const double sinW0 = sin (W0);
 211         const double cosW0 = cos (W0);
 212         const double alpha = sinW0 / (2.0 * Q);
 213         const double beta  = sqrt (A) / Q;
 214
 215         double _a0;
 216
 217         switch (type) {
 218                 case LowPass:
 219                         _b0 = (1.0 - cosW0) / 2.0;
 220                         _b1 =  1.0 - cosW0;
 221                         _b2 = (1.0 - cosW0) / 2.0;
 222                         _a0 =  1.0 + alpha;
 223                         _a1 = -2.0 * cosW0;
 224                         _a2 =  1.0 - alpha;
 225                         break;
 226
 227                 case HighPass:
 228                         _b0 =  (1.0 + cosW0) / 2.0;
 229                         _b1 = -(1.0 + cosW0);
 230                         _b2 =  (1.0 + cosW0) / 2.0;
 231                         _a0 =   1.0 + alpha;
 232                         _a1 =  -2.0 * cosW0;
 233                         _a2 =   1.0 - alpha;
 234                         break;
 235
 236                 case BandPassSkirt: /* Constant skirt gain, peak gain = Q */
 237                         _b0 =  sinW0 / 2.0;
 238                         _b1 =  0.0;
 239                         _b2 = -sinW0 / 2.0;
 240                         _a0 =  1.0 + alpha;
 241                         _a1 = -2.0 * cosW0;
 242                         _a2 =  1.0 - alpha;
 243                         break;
 244
 245                 case BandPass0dB: /* Constant 0 dB peak gain */
 246                         _b0 =  alpha;
 247                         _b1 =  0.0;
 248                         _b2 = -alpha;
 249                         _a0 =  1.0 + alpha;
 250                         _a1 = -2.0 * cosW0;
 251                         _a2 =  1.0 - alpha;
 252                         break;
 253
 254                 case Notch:
 255                         _b0 =  1.0;
 256                         _b1 = -2.0 * cosW0;
 257                         _b2 =  1.0;
 258                         _a0 =  1.0 + alpha;
 259                         _a1 = -2.0 * cosW0;
 260                         _a2 =  1.0 - alpha;
 261                         break;
 262
 263                 case AllPass:
 264                         _b0 =  1.0 - alpha;
 265                         _b1 = -2.0 * cosW0;
 266                         _b2 =  1.0 + alpha;
 267                         _a0 =  1.0 + alpha;
 268                         _a1 = -2.0 * cosW0;
 269                         _a2 =  1.0 - alpha;
 270                         break;
 271
 272                 case Peaking:
 273                         _b0 =  1.0 + (alpha * A);
 274                         _b1 = -2.0 * cosW0;
 275                         _b2 =  1.0 - (alpha * A);
 276                         _a0 =  1.0 + (alpha / A);
 277                         _a1 = -2.0 * cosW0;
 278                         _a2 =  1.0 - (alpha / A);
 279                         break;
 280
 281                 case LowShelf:
 282                         _b0 =         A * ((A + 1) - ((A - 1) * cosW0) + (beta * sinW0));
 283                         _b1 = (2.0 * A) * ((A - 1) - ((A + 1) * cosW0));
 284                         _b2 =         A * ((A + 1) - ((A - 1) * cosW0) - (beta * sinW0));
 285                         _a0 =              (A + 1) + ((A - 1) * cosW0) + (beta * sinW0);
 286                         _a1 =      -2.0 * ((A - 1) + ((A + 1) * cosW0));
 287                         _a2 =              (A + 1) + ((A - 1) * cosW0) - (beta * sinW0);
 288                         break;
 289
 290                 case HighShelf:
 291                         _b0 =          A * ((A + 1) + ((A - 1) * cosW0) + (beta * sinW0));
 292                         _b1 = -(2.0 * A) * ((A - 1) + ((A + 1) * cosW0));
 293                         _b2 =          A * ((A + 1) + ((A - 1) * cosW0) - (beta * sinW0));
 294                         _a0 =               (A + 1) - ((A - 1) * cosW0) + (beta * sinW0);
 295                         _a1 =        2.0 * ((A - 1) - ((A + 1) * cosW0));
 296                         _a2 =               (A + 1) - ((A - 1) * cosW0) - (beta * sinW0);
 297                         break;
 298                 default:
 299                         abort(); /*NOTREACHED*/
 300                         break;
 301         }
 302
 303         _b0 /= _a0;
 304         _b1 /= _a0;
 305         _b2 /= _a0;
 306         _a1 /= _a0;
 307         _a2 /= _a0;
 308 }
 309
 310 float
 311 Biquad::dB_at_freq (float freq) const
 312 {
 313         const double W0 = (2.0 * M_PI * freq) / _rate;
 314         const float c1 = cosf (W0);
 315         const float s1 = sinf (W0);
 316
 317         const float A = _b0 + _b2;
 318         const float B = _b0 - _b2;
 319         const float C = 1.0 + _a2;
 320         const float D = 1.0 - _a2;
 321
 322         const float a = A * c1 + _b1;
 323         const float b = B * s1;
 324         const float c = C * c1 + _a1;
 325         const float d = D * s1;
 326
 327 #define SQUARE(x) ( (x) * (x) )
 328         float rv = 20.f * log10f (sqrtf ((SQUARE(a) + SQUARE(b)) * (SQUARE(c) + SQUARE(d))) / (SQUARE(c) + SQUARE(d)));
 329         if (!isfinite_local (rv)) { rv = 0; }
 330         return std::min (120.f, std::max(-120.f, rv));
 331 }
 332
 333
 334 Glib::Threads::Mutex FFTSpectrum::fft_planner_lock;
 335
 336 FFTSpectrum::FFTSpectrum (uint32_t window_size, double rate)
 337         : hann_window (0)
 338 {
 339         init (window_size, rate);
 340 }
 341
 342 FFTSpectrum::~FFTSpectrum ()
 343 {
 344         {
 345                 Glib::Threads::Mutex::Lock lk (fft_planner_lock);
 346                 fftwf_destroy_plan (_fftplan);
 347         }
 348         fftwf_free (_fft_data_in);
 349         fftwf_free (_fft_data_out);
 350         free (_fft_power);
 351         free (hann_window);
 352 }
 353
 354 void
 355 FFTSpectrum::init (uint32_t window_size, double rate)
 356 {
 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 }