simplealsa.d

   1 /*
   2  * Copyright (c) 2016, Ketmar // Invisible Vector
   3  *
   4  * Redistribution and use in source and binary forms, with or without
   5  * modification, are permitted provided that the following conditions
   6  * are met:
   7  *
   8  * - Redistributions of source code must retain the above copyright
   9  * notice, this list of conditions and the following disclaimer.
  10  *
  11  * - Redistributions in binary form must reproduce the above copyright
  12  * notice, this list of conditions and the following disclaimer in the
  13  * documentation and/or other materials provided with the distribution.
  14  *
  15  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  16  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  17  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  18  * A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION
  19  * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  20  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  21  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  22  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  23  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  24  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  25  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  26  */
  27 /** simple, yet useful blocking ALSA player. can do resampling and has 39-band equalizer */
  28 module iv.simplealsa /*is aliced*/;
  29 private:
  30
  31 //version = simplealsa_writefile;
  32 //version = eq_debug;
  33 //version = SIALSA_X86_TRICK;
  34
  35
  36 import iv.alice;
  37 import iv.alsa;
  38 import iv.follin.resampler;
  39 import iv.follin.utils;
  40 version(simplealsa_writefile) import iv.vfs;
  41
  42
  43 // ////////////////////////////////////////////////////////////////////////// //
  44 public __gshared string alsaDevice = "default"; /// output device
  45 public __gshared ubyte alsaRQuality = SpeexResampler.Music; /// resampling quality (if required); [0..10]; default is 8
  46 public __gshared int[EQ_MAX_BANDS] alsaEqBands = 0; /// [-20..20]
  47 public __gshared int alsaGain = 0; /// sound gain, in %
  48 public __gshared uint alsaLatencyms = 100; /// output latency, in milliseconds
  49 public __gshared bool alsaEnableResampling = true; /// set to `false` to disable resampling (sound can be distorted)
  50 public __gshared bool alsaEnableEqualizer = false; /// set to `false` to disable equalizer
  51
  52
  53 // ////////////////////////////////////////////////////////////////////////// //
  54 public @property bool alsaIsOpen () nothrow @trusted @nogc { return (pcm !is null); } ///
  55 public @property uint alsaRate () nothrow @trusted @nogc { return srate; } ///
  56 public @property uint alsaRealRate () nothrow @trusted @nogc { return realsrate; } ///
  57 public @property ubyte alsaChannels () nothrow @trusted @nogc { return cast(ubyte)xxoutchans; } ///
  58
  59
  60 // ////////////////////////////////////////////////////////////////////////// //
  61 /// find best (native, if output device is "default") supported sampling rate, or 0 on error
  62 public uint alsaGetBestSampleRate (uint wantedRate) {
  63   import std.internal.cstring : tempCString;
  64
  65   if (wantedRate == 0) wantedRate = 44110;
  66
  67   snd_pcm_t* pcm;
  68   snd_pcm_hw_params_t* hwparams;
  69
  70   auto err = snd_pcm_open(&pcm, alsaDevice.tempCString, SND_PCM_STREAM_PLAYBACK, SND_PCM_NONBLOCK);
  71   if (err < 0) return 0;
  72   scope(exit) snd_pcm_close(pcm);
  73
  74   err = snd_pcm_hw_params_malloc(&hwparams);
  75   if (err < 0) return 0;
  76   scope(exit) snd_pcm_hw_params_free(hwparams);
  77
  78   err = snd_pcm_hw_params_any(pcm, hwparams);
  79   if (err < 0) return 0;
  80
  81   //printf("Device: %s (type: %s)\n", device_name, snd_pcm_type_name(snd_pcm_type(pcm)));
  82
  83   if (snd_pcm_hw_params_test_rate(pcm, hwparams, wantedRate, 0) == 0) return wantedRate;
  84
  85   uint min, max;
  86
  87   err = snd_pcm_hw_params_get_rate_min(hwparams, &min, null);
  88   if (err < 0) return 0;
  89
  90   err = snd_pcm_hw_params_get_rate_max(hwparams, &max, null);
  91   if (err < 0) return 0;
  92
  93   if (wantedRate < min) return min;
  94   if (wantedRate > max) return max;
  95
  96   for (int delta = 1; delta < wantedRate; ++delta) {
  97     if (wantedRate-delta < min && wantedRate+delta > max) break;
  98     if (wantedRate-delta > min) {
  99       if (snd_pcm_hw_params_test_rate(pcm, hwparams, wantedRate-delta, 0) == 0) return wantedRate-delta;
 100     }
 101     if (wantedRate+delta < max) {
 102       if (snd_pcm_hw_params_test_rate(pcm, hwparams, wantedRate+delta, 0) == 0) return wantedRate+delta;
 103     }
 104   }
 105   return (wantedRate-min < max-wantedRate ? min : max);
 106 }
 107
 108
 109 // ////////////////////////////////////////////////////////////////////////// //
 110 version(simplealsa_writefile) VFile fo;
 111 __gshared snd_pcm_t* pcm;
 112
 113 __gshared SpeexResampler srb;
 114
 115 __gshared uint srate, realsrate;
 116
 117 __gshared int lastGain = 0;
 118 __gshared int lastEqSRate = 0;
 119 __gshared int[EQ_MAX_BANDS] lastEqBands = -666;
 120
 121
 122 enum XXBUF_SIZE = 4096;
 123 __gshared ubyte[XXBUF_SIZE*4] xxbuffer; // just in case
 124 __gshared uint xxbufused;
 125 __gshared uint xxoutchans;
 126
 127
 128 // ////////////////////////////////////////////////////////////////////////// //
 129 void outSoundInit (uint chans) {
 130   if (chans < 1 || chans > 2) assert(0, "invalid number of channels");
 131   xxbufused = 0;
 132   xxoutchans = chans;
 133 }
 134
 135
 136 void outSoundFlushX (const(void)* buf, uint bytes) {
 137   version(simplealsa_writefile) {
 138     auto bb = cast(const(ubyte)*)buf;
 139     fo.rawWriteExact(bb[0..bytes]);
 140   } else {
 141     auto bb = cast(const(short)*)buf;
 142     auto fleft = bytes/(2*xxoutchans);
 143     while (fleft > 0) {
 144       auto frames = snd_pcm_writei(pcm, bb, fleft);
 145       if (frames < 0) {
 146         frames = snd_pcm_recover(pcm, cast(int)frames, 0);
 147         if (frames < 0) {
 148           //import core.stdc.stdio : printf;
 149           //printf("snd_pcm_writei failed: %s\n", snd_strerror(cast(int)frames));
 150         }
 151       } else {
 152         bb += cast(uint)frames*xxoutchans;
 153         fleft -= cast(uint)frames;
 154       }
 155     }
 156   }
 157 }
 158
 159
 160 //TODO: optimize code to avoid multiple float<->short conversions
 161 void outSoundFlush () {
 162   __gshared float[XXBUF_SIZE] rsfbufi = 0;
 163   __gshared float[XXBUF_SIZE] rsfbufo = 0;
 164
 165   if (xxbufused == 0) return;
 166   assert(xxbufused%(2*xxoutchans) == 0);
 167   auto smpCount = xxbufused/2;
 168   xxbufused = 0;
 169   //{ import core.stdc.stdio; printf("smpCount: %u\n", cast(uint)smpCount); }
 170
 171   bool didFloat = false;
 172   short* b = cast(short*)xxbuffer.ptr;
 173   // do gain
 174   if (alsaGain) {
 175     didFloat = true;
 176     tflShort2Float(b[0..smpCount], rsfbufi[0..smpCount]);
 177     immutable float gg = alsaGain/100.0f;
 178     foreach (ref float v; rsfbufi[0..smpCount]) v += v*gg;
 179     //tflFloat2Short(rsfbufi[0..smpCount], b[0..smpCount]);
 180   }
 181
 182   // equalizer
 183   bool doeq = false;
 184   if (alsaEnableEqualizer) foreach (int v; alsaEqBands[]) if (v != 0) { doeq = true; break; }
 185   //doeq = alsaEnableEqualizer;
 186
 187   /*
 188   if (doeq && alsaEnableEqualizer) {
 189     if (!didFloat) {
 190       didFloat = true;
 191       tflShort2Float(b[0..smpCount], rsfbufi[0..smpCount]);
 192     }
 193     mbeql.bands[] = alsaEqBands[];
 194     if (xxoutchans == 1) {
 195       mbeql.run(rsfbufo[0..smpCount], rsfbufi[0..smpCount]);
 196     } else {
 197       mbeqr.bands[] = alsaEqBands[];
 198       mbeql.run(rsfbufo[0..smpCount], rsfbufi[0..smpCount], 2, 0);
 199       mbeqr.run(rsfbufo[0..smpCount], rsfbufi[0..smpCount], 2, 1);
 200     }
 201     rsfbufi[0..smpCount] = rsfbufo[0..smpCount];
 202     //tflFloat2Short(rsfbufo[0..smpCount], b[0..smpCount]);
 203   }
 204   */
 205
 206   void doEqualizing (short* buf, uint samples, uint srate) {
 207     if (doeq && samples) {
 208       if (srate != lastEqSRate) {
 209         initEqIIR(srate);
 210         lastEqSRate = srate;
 211         lastEqBands[] = -666;
 212         //{ import core.stdc.stdio; printf("equalizer reinited, srate: %u; chans: %u\n", srate, cast(uint)xxoutchans); }
 213       }
 214       foreach (immutable bidx, int bv; alsaEqBands[]) {
 215         if (bv < -20) bv = -20; else if (bv > 20) bv = 20;
 216         if (bv != lastEqBands.ptr[bidx]) {
 217           lastEqBands.ptr[bidx] = bv;
 218           double v = 0.03*bv+0.000999999*bv*bv;
 219           //{ import core.stdc.stdio; printf("  band #%u; value=%d; v=%g\n", cast(uint)bidx, bv, v); }
 220           set_gain(cast(int)bidx, 0/*chan*/, v);
 221           set_gain(cast(int)bidx, 1/*chan*/, v);
 222         }
 223       }
 224       iir(buf, cast(int)samples, xxoutchans);
 225     }
 226   }
 227
 228   //{ import core.stdc.stdio; printf("smpCount: %u\n", cast(uint)smpCount); }
 229   // need resampling?
 230   if (srate == realsrate || !alsaEnableResampling) {
 231     // easy deal, no resampling required
 232     if (didFloat) tflFloat2Short(rsfbufi[0..smpCount], b[0..smpCount]);
 233     doEqualizing(b, smpCount, srate);
 234     outSoundFlushX(b, smpCount*2);
 235   } else {
 236     // oops, must resample
 237     SpeexResampler.Data srbdata;
 238     if (!didFloat) {
 239       didFloat = true;
 240       tflShort2Float(b[0..smpCount], rsfbufi[0..smpCount]);
 241     }
 242     uint inpos = 0;
 243     for (;;) {
 244       srbdata = srbdata.init; // just in case
 245       srbdata.dataIn = rsfbufi[inpos..smpCount];
 246       srbdata.dataOut = rsfbufo[];
 247       if (srb.process(srbdata) != 0) assert(0, "resampling error");
 248       //{ import core.stdc.stdio; printf("inpos=%u; smpCount=%u; iu=%u; ou=%u\n", cast(uint)inpos, cast(uint)smpCount, cast(uint)srbdata.inputSamplesUsed, cast(uint)srbdata.outputSamplesUsed); }
 249       if (srbdata.outputSamplesUsed) {
 250         tflFloat2Short(rsfbufo[0..srbdata.outputSamplesUsed], b[0..srbdata.outputSamplesUsed]);
 251         doEqualizing(b, srbdata.outputSamplesUsed, realsrate);
 252         outSoundFlushX(b, srbdata.outputSamplesUsed*2);
 253       } else {
 254         // no data consumed, no data produced, so we're done
 255         if (inpos >= smpCount) break;
 256       }
 257       inpos += cast(uint)srbdata.inputSamplesUsed;
 258     }
 259   }
 260   //{ import core.stdc.stdio; printf("OK (%u)\n", cast(uint)xxbufused); }
 261 }
 262
 263
 264 void outSoundS (const(void)* buf, uint bytes) {
 265   //{ import core.stdc.stdio; printf("outSoundS: %u\n", bytes); }
 266   auto src = cast(const(ubyte)*)buf;
 267   while (bytes > 0) {
 268     while (bytes > 0 && xxbufused < XXBUF_SIZE) {
 269       xxbuffer.ptr[xxbufused++] = *src++;
 270       --bytes;
 271     }
 272     if (xxbufused == XXBUF_SIZE) outSoundFlush();
 273   }
 274   //{ import core.stdc.stdio; printf("outSoundS: DONE\n"); }
 275 }
 276
 277
 278 void outSoundF (const(void)* buf, uint bytes) {
 279   __gshared short[XXBUF_SIZE] cvtbuffer;
 280   auto len = bytes/float.sizeof;
 281   assert(len <= cvtbuffer.length);
 282   tflFloat2Short((cast(const(float)*)buf)[0..len], cvtbuffer[0..len]);
 283   outSoundS(cvtbuffer.ptr, cast(uint)(len*2));
 284 }
 285
 286
 287 // ////////////////////////////////////////////////////////////////////////// //
 288 /// shutdown player
 289 public void alsaShutdown (bool immediate=false) {
 290   if (pcm !is null) {
 291     if (immediate) {
 292       snd_pcm_drop(pcm);
 293     } else {
 294       if (xxbufused > 0) outSoundFlush();
 295       snd_pcm_drain(pcm);
 296     }
 297     snd_pcm_close(pcm);
 298     pcm = null;
 299   }
 300   srate = realsrate = 0;
 301   xxoutchans = 0;
 302   version(simplealsa_writefile) fo.close();
 303 }
 304
 305
 306 // ////////////////////////////////////////////////////////////////////////// //
 307 /// (re)initialize player; return success flag
 308 public bool alsaInit (uint asrate, ubyte chans) {
 309   import std.internal.cstring : tempCString;
 310
 311   alsaShutdown(true);
 312   fuck_alsa_messages();
 313
 314   if (asrate < 1024 || asrate > 96000) return false;
 315   if (chans < 1 || chans > 2) return false;
 316
 317   srate = asrate;
 318   if (asrate == 44100 || asrate == 48000) {
 319     realsrate = alsaGetBestSampleRate(asrate);
 320   } else {
 321     realsrate = alsaGetBestSampleRate(48000);
 322   }
 323   if (realsrate == 0) return false; // alas
 324
 325   if (realsrate != srate) {
 326     srb.setup(chans, srate, realsrate, alsaRQuality);
 327   }
 328
 329   outSoundInit(chans);
 330
 331   int err;
 332
 333   if ((err = snd_pcm_open(&pcm, alsaDevice.tempCString, SND_PCM_STREAM_PLAYBACK, 0)) < 0) {
 334     //import core.stdc.stdlib : exit, EXIT_FAILURE;
 335     //conwriteln("Playback open error for device '%s': %s", device, snd_strerror(err));
 336     //exit(EXIT_FAILURE);
 337     return false;
 338   }
 339   //scope(exit) snd_pcm_close(pcm);
 340
 341   if ((err = snd_pcm_set_params(pcm, SND_PCM_FORMAT_S16_LE, SND_PCM_ACCESS_RW_INTERLEAVED, chans, /*sio.rate*/realsrate, 1, /*500000*//*20000*/alsaLatencyms*1000)) < 0) {
 342     //import core.stdc.stdlib : exit, EXIT_FAILURE;
 343     //conwriteln("Playback open error: %s", snd_strerror(err));
 344     //exit(EXIT_FAILURE);
 345     snd_pcm_close(pcm);
 346     pcm = null;
 347     return false;
 348   }
 349
 350   version(simplealsa_writefile) fo = VFile("./zout.raw", "w");
 351
 352   return true;
 353 }
 354
 355
 356 // ////////////////////////////////////////////////////////////////////////// //
 357 /// write (interleaved) buffer
 358 public void alsaWriteShort (const(short)[] buf) {
 359   if (pcm is null || buf.length == 0) return;
 360   if (buf.length >= 1024*1024) assert(0, "too much");
 361   outSoundS(buf.ptr, cast(uint)(buf.length*buf[0].sizeof));
 362 }
 363
 364
 365 /// write (interleaved) buffer
 366 public void alsaWriteFloat (const(float)[] buf) {
 367   if (pcm is null || buf.length == 0) return;
 368   if (buf.length >= 1024*1024) assert(0, "too much");
 369   outSoundF(buf.ptr, cast(uint)(buf.length*buf[0].sizeof));
 370 }
 371
 372
 373 // ////////////////////////////////////////////////////////////////////////// //
 374 /*
 375  *   PCM time-domain equalizer
 376  *
 377  *   Copyright (C) 2002-2005  Felipe Rivera <liebremx at users.sourceforge.net>
 378  *
 379  *   This program is free software; you can redistribute it and/or modify
 380  *   it under the terms of the GNU General Public License as published by
 381  *   the Free Software Foundation; either version 2 of the License, or
 382  *   (at your option) any later version.
 383  *
 384  *   This program is distributed in the hope that it will be useful,
 385  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 386  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 387  *   GNU General Public License for more details.
 388  *
 389  *   You should have received a copy of the GNU General Public License
 390  *   along with this program; if not, write to the Free Software
 391  *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 392  *
 393  *   $Id: iir.h,v 1.12 2005/10/17 01:57:59 liebremx Exp $
 394  */
 395 private:
 396
 397 /*public*/ enum EQ_CHANNELS = 2; // 6
 398 public enum EQ_MAX_BANDS = 10;
 399
 400
 401 /* Coefficients entry */
 402 struct sIIRCoefficients {
 403   float beta;
 404   float alpha;
 405   float gamma;
 406   //float dummy; // Word alignment
 407 }
 408
 409 __gshared float[EQ_CHANNELS] preamp = 1.0; // Volume gain; values should be between 0.0 and 1.0
 410 __gshared sIIRCoefficients* eqiirCoeffs;
 411 //__gshared int bandCount;
 412 enum bandCount = EQ_MAX_BANDS;
 413
 414
 415 // Init the filters
 416 /*public*/ void initEqIIR (uint srate) nothrow @trusted @nogc {
 417   //bandCount = EQ_MAX_BANDS;
 418   auto br = BandRec(srate);
 419   calc_coeffs(br);
 420   eqiirCoeffs = br.coeffs;
 421   clean_history();
 422 }
 423
 424
 425
 426 /***************************
 427  * IIR filter coefficients *
 428  ***************************/
 429 __gshared sIIRCoefficients[10] iir_cfx;
 430
 431
 432 /******************************************************************
 433  * Definitions and data structures to calculate the coefficients
 434  ******************************************************************/
 435 static immutable double[10] band_f011k = [ 31, 62, 125, 250, 500, 1000, 2000, 3000, 4000, 5500 ];
 436 static immutable double[10] band_f022k = [ 31, 62, 125, 250, 500, 1000, 2000, 4000, 8000, 11000 ];
 437 static immutable double[10] band_f010 = [ 31, 62, 125, 250, 500, 1000, 2000, 4000, 8000, 16000 ];
 438 static immutable double[10] band_original_f010 = [ 60, 170, 310, 600, 1000, 3000, 6000, 12000, 14000, 16000 ];
 439 /*
 440 static immutable double[15] band_f015 = [ 25,40,63,100,160,250,400,630,1000,1600,2500,4000,6300,10000,16000 ];
 441 static immutable double[25] band_f025 = [ 20,31.5,40,50,80,100,125,160,250,315,400,500,800,1000,1250,1600,2500,3150,4000,5000,8000,10000,12500,16000,20000 ];
 442 static immutable double[31] band_f031 = [ 20,25,31.5,40,50,63,80,100,125,160,200,250,315,400,500,630,800,1000,1250,1600,2000,2500,3150,4000,5000,6300,8000,10000,12500,16000,20000 ];
 443 */
 444
 445 struct BandRec {
 446   sIIRCoefficients* coeffs;
 447   immutable(double)* cfs;
 448   double octave = 1.0;
 449   //int bandCount;
 450   enum bandCount = EQ_MAX_BANDS;
 451   double sfreq;
 452
 453   this (int samplerate) nothrow @trusted @nogc {
 454     coeffs = iir_cfx.ptr;
 455          if (samplerate <= 11025) cfs = band_f011k.ptr;
 456     else if (samplerate <= 22050) cfs = band_f022k.ptr;
 457     else if (samplerate <= 48000) cfs = band_original_f010.ptr;
 458     else cfs = band_f010.ptr;
 459     sfreq = samplerate;
 460   }
 461 }
 462
 463 /+
 464 __gshared BandRec[13] bands = [
 465   BandRec(iir_cf10_11k_11025.ptr,     band_f011k.ptr,         1.0,     10, 11025.0 ),
 466   BandRec(iir_cf10_22k_22050.ptr,     band_f022k.ptr,         1.0,     10, 22050.0 ),
 467   BandRec(iir_cforiginal10_44100.ptr, band_original_f010.ptr, 1.0,     10, 44100.0 ),
 468   BandRec(iir_cforiginal10_48000.ptr, band_original_f010.ptr, 1.0,     10, 48000.0 ),
 469   BandRec(iir_cf10_96000.ptr,         band_f010.ptr,          1.0,     10, 96000.0 ),
 470   /*
 471   BandRec(iir_cf10_44100.ptr,         band_f010.ptr,          1.0,     10, 44100.0 ),
 472   BandRec(iir_cf10_48000.ptr,         band_f010.ptr,          1.0,     10, 48000.0 ),
 473   BandRec(iir_cf15_44100.ptr,         band_f015.ptr,          2.0/3.0, 15, 44100.0 ),
 474   BandRec(iir_cf15_48000.ptr,         band_f015.ptr,          2.0/3.0, 15, 48000.0 ),
 475   BandRec(iir_cf25_44100.ptr,         band_f025.ptr,          1.0/3.0, 25, 44100.0 ),
 476   BandRec(iir_cf25_48000.ptr,         band_f025.ptr,          1.0/3.0, 25, 48000.0 ),
 477   BandRec(iir_cf31_44100.ptr,         band_f031.ptr,          1.0/3.0, 31, 44100.0 ),
 478   BandRec(iir_cf31_48000.ptr,         band_f031.ptr,          1.0/3.0, 31, 48000.0 ),
 479   */
 480 ];
 481
 482 shared static this () { calc_coeffs(); }
 483 +/
 484
 485
 486 import std.math : PI, SQRT2;
 487
 488 enum GAIN_F0 = 1.0;
 489 enum GAIN_F1 = GAIN_F0/SQRT2;
 490
 491 double TETA (double sfreq, double f) nothrow @trusted @nogc { return 2*PI*cast(double)f/ /*bands[n].*/sfreq; }
 492 double TWOPOWER (double value) nothrow @trusted @nogc { return value*value; }
 493
 494 auto BETA2 (double tf0, double tf) nothrow @trusted @nogc {
 495   import std.math : cos, sin;
 496   return
 497     (TWOPOWER(GAIN_F1)*TWOPOWER(cos(tf0))
 498      - 2.0 * TWOPOWER(GAIN_F1) * cos(tf) * cos(tf0)
 499      + TWOPOWER(GAIN_F1)
 500      - TWOPOWER(GAIN_F0) * TWOPOWER(sin(tf)));
 501 }
 502
 503 auto BETA1 (double tf0, double tf) nothrow @trusted @nogc {
 504   import std.math : cos, sin;
 505   return
 506     (2.0 * TWOPOWER(GAIN_F1) * TWOPOWER(cos(tf))
 507      + TWOPOWER(GAIN_F1) * TWOPOWER(cos(tf0))
 508      - 2.0 * TWOPOWER(GAIN_F1) * cos(tf) * cos(tf0)
 509      - TWOPOWER(GAIN_F1) + TWOPOWER(GAIN_F0) * TWOPOWER(sin(tf)));
 510 }
 511
 512 auto BETA0 (double tf0, double tf) nothrow @trusted @nogc {
 513   import std.math : cos, sin;
 514   return
 515     (0.25 * TWOPOWER(GAIN_F1) * TWOPOWER(cos(tf0))
 516      - 0.5 * TWOPOWER(GAIN_F1) * cos(tf) * cos(tf0)
 517      + 0.25 * TWOPOWER(GAIN_F1)
 518      - 0.25 * TWOPOWER(GAIN_F0) * TWOPOWER(sin(tf)));
 519 }
 520
 521
 522 auto GAMMA (double beta, double tf0) nothrow @trusted @nogc { import std.math : cos; return (0.5+beta)*cos(tf0); }
 523 auto ALPHA (double beta) nothrow @trusted @nogc { return (0.5-beta)/2.0; }
 524
 525 /* Get the coeffs for a given number of bands and sampling frequency */
 526 /*
 527 sIIRCoefficients* get_coeffs (uint sfreq) nothrow @trusted @nogc {
 528   switch (sfreq) {
 529     case 11025: return iir_cf10_11k_11025.ptr;
 530     case 22050: return eqiirCoeffs = iir_cf10_22k_22050.ptr;
 531     case 48000: return eqiirCoeffs = iir_cforiginal10_48000.ptr;
 532     case 96000: return eqiirCoeffs = iir_cf10_96000.ptr;
 533     default: break;
 534   }
 535   return null;
 536 }
 537 */
 538
 539
 540 /* Get the freqs at both sides of F0. These will be cut at -3dB */
 541 void find_f1_and_f2 (double f0, double octave_percent, double* f1, double* f2) nothrow @trusted @nogc {
 542   import std.math : pow;
 543   double octave_factor = pow(2.0, octave_percent/2.0);
 544   *f1 = f0/octave_factor;
 545   *f2 = f0*octave_factor;
 546 }
 547
 548
 549 /* Find the quadratic root
 550  * Always return the smallest root */
 551 bool find_root (double a, double b, double c, double* x0) nothrow @trusted @nogc {
 552   import std.math : sqrt;
 553   immutable double k = c-((b*b)/(4.0*a));
 554   if (-(k/a) < 0.0) return false;
 555   immutable double h = -(b/(2.0*a));
 556   *x0 = h-sqrt(-(k/a));
 557   immutable double x1 = h+sqrt(-(k/a));
 558   if (x1 < *x0) *x0 = x1;
 559   return true;
 560 }
 561
 562
 563 /* Calculate all the coefficients as specified in the bands[] array */
 564 void calc_coeffs (ref BandRec band) nothrow @trusted @nogc {
 565   immutable(double)* freqs = band.cfs;
 566   for (int i = 0; i < band.bandCount; ++i) {
 567     double f1 = void, f2 = void;
 568     double x0 = void;
 569     /* Find -3dB frequencies for the center freq */
 570     find_f1_and_f2(freqs[i], band.octave, &f1, &f2);
 571     /* Find Beta */
 572     if (find_root(
 573           BETA2(TETA(band.sfreq, freqs[i]), TETA(band.sfreq, f1)),
 574           BETA1(TETA(band.sfreq, freqs[i]), TETA(band.sfreq, f1)),
 575           BETA0(TETA(band.sfreq, freqs[i]), TETA(band.sfreq, f1)),
 576           &x0))
 577     {
 578       /* Got a solution, now calculate the rest of the factors */
 579       /* Take the smallest root always (find_root returns the smallest one)
 580        *
 581        * NOTE: The IIR equation is
 582        *  y[n] = 2 * (alpha*(x[n]-x[n-2]) + gamma*y[n-1] - beta*y[n-2])
 583        *  Now the 2 factor has been distributed in the coefficients
 584        */
 585       /* Now store the coefficients */
 586       band.coeffs[i].beta = 2.0*x0;
 587       band.coeffs[i].alpha = 2.0*ALPHA(x0);
 588       band.coeffs[i].gamma = 2.0*GAMMA(x0, TETA(band.sfreq, freqs[i]));
 589       version(eq_debug) {
 590         import core.stdc.stdio;
 591         printf("Freq[%d]: %f. Beta: %.10e Alpha: %.10e Gamma %.10e\n", i, freqs[i], band.coeffs[i].beta, band.coeffs[i].alpha, band.coeffs[i].gamma);
 592       }
 593     } else {
 594       /* Shouldn't happen */
 595       band.coeffs[i].beta = 0.0;
 596       band.coeffs[i].alpha = 0.0;
 597       band.coeffs[i].gamma = 0.0;
 598       import core.stdc.stdio;
 599       printf("  **** Where are the roots?\n");
 600     }
 601   }
 602 }
 603
 604
 605 alias sample_t = double;
 606
 607 /*
 608  * Normal FPU implementation data structures
 609  */
 610 /* Coefficient history for the IIR filter */
 611 struct sXYData {
 612   sample_t[3] x = 0; /* x[n], x[n-1], x[n-2] */
 613   sample_t[3] y = 0; /* y[n], y[n-1], y[n-2] */
 614   //sample_t dummy1; // Word alignment
 615   //sample_t dummy2;
 616 }
 617
 618
 619 //static sXYData data_history[EQ_MAX_BANDS][EQ_CHANNELS];
 620 //static sXYData data_history2[EQ_MAX_BANDS][EQ_CHANNELS];
 621 //float gain[EQ_MAX_BANDS][EQ_CHANNELS];
 622
 623 __gshared sXYData[EQ_CHANNELS][EQ_MAX_BANDS] data_history;
 624 __gshared sXYData[EQ_CHANNELS][EQ_MAX_BANDS] data_history2;
 625 __gshared float[EQ_CHANNELS][EQ_MAX_BANDS] gain;
 626
 627 shared static this () {
 628   foreach (immutable bn; 0..EQ_MAX_BANDS) {
 629     foreach (immutable cn; 0..EQ_CHANNELS) {
 630       gain[bn][cn] = 0;
 631     }
 632   }
 633 }
 634
 635 /* random noise */
 636 __gshared sample_t[256] dither;
 637 __gshared int di;
 638
 639 /* Indexes for the history arrays
 640  * These have to be kept between calls to this function
 641  * hence they are static */
 642 __gshared int iirI = 2, iirJ = 1, iirK = 0;
 643
 644
 645 /*public*/ void set_gain (int index, int chn, float val) nothrow @trusted @nogc {
 646   gain[index][chn] = val;
 647 }
 648
 649
 650 void clean_history () nothrow @trusted @nogc {
 651   //import core.stdc.string : memset;
 652   // Zero the history arrays
 653   //memset(data_history.ptr, 0, sXYData.sizeof * EQ_MAX_BANDS * EQ_CHANNELS);
 654   //memset(data_history2.ptr, 0, sXYData.sizeof * EQ_MAX_BANDS * EQ_CHANNELS);
 655   foreach (immutable bn; 0..EQ_MAX_BANDS) {
 656     foreach (immutable cn; 0..EQ_CHANNELS) {
 657       data_history[bn][cn] = sXYData.init;
 658       data_history2[bn][cn] = sXYData.init;
 659       //gain[bn][cn] = 0;
 660     }
 661   }
 662   import std.random : Xorshift32;
 663   //for (n = 0; n < 256; n++) dither[n] = (uniform!"[)"(0, 4)) - 2;
 664   auto xe = Xorshift32(666);
 665   dither[] = 0;
 666   for (int n = 0; n < 256; ++n) { int t = (xe.front%4)-2; dither.ptr[n] = cast(sample_t)t; xe.popFront(); }
 667   //{ import core.stdc.stdio; for (int n = 0; n < 256; ++n) printf("%d: %g\n", n, dither.ptr[n]); }
 668   //dither[] = 0;
 669   //for (int n = 0; n < 256; ++n) dither.ptr[n] = n%4-2;
 670   di = 0;
 671   iirI = 2;
 672   iirJ = 1;
 673   iirK = 0;
 674 }
 675
 676
 677 // input: 16-bit samples, interleaved; length in BYTES
 678 /*public*/ void iir (short* data, int smplength, int nch) nothrow @trusted @nogc {
 679   if (eqiirCoeffs is null) return;
 680
 681   /**
 682    * IIR filter equation is
 683    * y[n] = 2 * (alpha*(x[n]-x[n-2]) + gamma*y[n-1] - beta*y[n-2])
 684    *
 685    * NOTE: The 2 factor was introduced in the coefficients to save
 686    *      a multiplication
 687    *
 688    * This algorithm cascades two filters to get nice filtering
 689    * at the expense of extra CPU cycles
 690    */
 691   for (int index = 0; index < smplength; index += nch) {
 692     // for each channel
 693     for (int channel = 0; channel < nch; ++channel) {
 694       sample_t pcm = *data*4.0;
 695
 696       // preamp gain
 697       pcm *= preamp[channel];
 698
 699       // add random noise
 700       pcm += dither.ptr[di];
 701
 702       sample_t outs = 0.0;
 703
 704       // for each band
 705       for (int band = 0; band < bandCount; ++band) {
 706         // store Xi(n)
 707         data_history.ptr[band].ptr[channel].x.ptr[iirI] = pcm;
 708         // calculate and store Yi(n)
 709         data_history.ptr[band].ptr[channel].y.ptr[iirI] = (
 710           //     = alpha * [x(n)-x(n-2)]
 711           eqiirCoeffs[band].alpha * ( data_history.ptr[band].ptr[channel].x.ptr[iirI]-data_history.ptr[band].ptr[channel].x.ptr[iirK])
 712           //     + gamma * y(n-1)
 713           + eqiirCoeffs[band].gamma * data_history.ptr[band].ptr[channel].y.ptr[iirJ]
 714           //     - beta * y(n-2)
 715           - eqiirCoeffs[band].beta * data_history.ptr[band].ptr[channel].y.ptr[iirK]
 716         );
 717         // the multiplication by 2.0 was 'moved' into the coefficients to save CPU cycles here
 718         // apply the gain
 719         outs += data_history.ptr[band].ptr[channel].y.ptr[iirI]*gain.ptr[band].ptr[channel]; // * 2.0;
 720       } // for each band
 721
 722       version(all) { //if (cfg.eq_extra_filtering)
 723         // filter the sample again
 724         for (int band = 0; band < bandCount; ++band) {
 725           // store Xi(n)
 726           data_history2.ptr[band].ptr[channel].x.ptr[iirI] = outs;
 727           // calculate and store Yi(n)
 728           data_history2.ptr[band].ptr[channel].y.ptr[iirI] = (
 729             // y(n) = alpha * [x(n)-x(n-2)]
 730             eqiirCoeffs[band].alpha * (data_history2.ptr[band].ptr[channel].x.ptr[iirI]-data_history2.ptr[band].ptr[channel].x.ptr[iirK])
 731             //       + gamma * y(n-1)
 732             + eqiirCoeffs[band].gamma * data_history2.ptr[band].ptr[channel].y.ptr[iirJ]
 733             //     - beta * y(n-2)
 734             - eqiirCoeffs[band].beta * data_history2.ptr[band].ptr[channel].y.ptr[iirK]
 735           );
 736           // apply the gain
 737           outs +=  data_history2.ptr[band].ptr[channel].y.ptr[iirI]*gain.ptr[band].ptr[channel];
 738         } // for each band
 739       }
 740
 741       /* Volume stuff
 742          Scale down original PCM sample and add it to the filters
 743          output. This substitutes the multiplication by 0.25
 744          Go back to use the floating point multiplication before the
 745          conversion to give more dynamic range
 746          */
 747       outs += pcm*0.25;
 748
 749       // remove random noise
 750       outs -= dither.ptr[di]*0.25;
 751
 752       // round and convert to integer
 753       version(X86) {
 754         import core.stdc.math : lrint;
 755         int tempgint = cast(int)lrint(outs);
 756         //int tempgint = cast(int)outs;
 757       } else {
 758         int tempgint = cast(int)outs;
 759       }
 760
 761       // limit the output
 762            if (tempgint < short.min) *data = short.min;
 763       else if (tempgint > short.max) *data = short.max;
 764       else *data = cast(short)tempgint;
 765       ++data;
 766     } // for each channel
 767
 768     // wrap around the indexes
 769     iirI = (iirI+1)%3;
 770     iirJ = (iirJ+1)%3;
 771     iirK = (iirK+1)%3;
 772     // random noise index
 773     di = (di+1)%256;
 774   }
 775 }