deps/libretro-common/audio/dsp_filters/iir.c

   1 /* Copyright  (C) 2010-2020 The RetroArch team
   2  *
   3  * ---------------------------------------------------------------------------------------
   4  * The following license statement only applies to this file (iir.c).
   5  * ---------------------------------------------------------------------------------------
   6  *
   7  * Permission is hereby granted, free of charge,
   8  * to any person obtaining a copy of this software and associated documentation files (the "Software"),
   9  * to deal in the Software without restriction, including without limitation the rights to
  10  * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software,
  11  * and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
  12  *
  13  * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
  14  *
  15  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
  16  * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  17  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
  18  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
  19  * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  20  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  21  */
  22
  23 #include <math.h>
  24 #include <stdlib.h>
  25 #include <string.h>
  26
  27 #include <retro_miscellaneous.h>
  28 #include <libretro_dspfilter.h>
  29 #include <string/stdstring.h>
  30
  31 #define sqr(a) ((a) * (a))
  32
  33 /* filter types */
  34 enum IIRFilter
  35 {
  36    LPF,        /* low pass filter */
  37    HPF,        /* High pass filter */
  38    BPCSGF,     /* band pass filter 1 */
  39    BPZPGF,     /* band pass filter 2 */
  40    APF,        /* Allpass filter*/
  41    NOTCH,      /* Notch Filter */
  42    RIAA_phono, /* RIAA record/tape deemphasis */
  43    PEQ,        /* Peaking band EQ filter */
  44    BBOOST,     /* Bassboost filter */
  45    LSH,        /* Low shelf filter */
  46    HSH,        /* High shelf filter */
  47    RIAA_CD     /* CD de-emphasis */
  48 };
  49
  50 struct iir_data
  51 {
  52    float b0, b1, b2;
  53    float a0, a1, a2;
  54
  55    struct
  56    {
  57       float xn1, xn2;
  58       float yn1, yn2;
  59    } l, r;
  60 };
  61
  62 static void iir_free(void *data)
  63 {
  64    free(data);
  65 }
  66
  67 static void iir_process(void *data, struct dspfilter_output *output,
  68       const struct dspfilter_input *input)
  69 {
  70    unsigned i;
  71    struct iir_data *iir = (struct iir_data*)data;
  72    float *out           = output->samples;
  73
  74    float b0             = iir->b0;
  75    float b1             = iir->b1;
  76    float b2             = iir->b2;
  77    float a0             = iir->a0;
  78    float a1             = iir->a1;
  79    float a2             = iir->a2;
  80
  81    float xn1_l          = iir->l.xn1;
  82    float xn2_l          = iir->l.xn2;
  83    float yn1_l          = iir->l.yn1;
  84    float yn2_l          = iir->l.yn2;
  85
  86    float xn1_r          = iir->r.xn1;
  87    float xn2_r          = iir->r.xn2;
  88    float yn1_r          = iir->r.yn1;
  89    float yn2_r          = iir->r.yn2;
  90
  91    output->samples      = input->samples;
  92    output->frames       = input->frames;
  93
  94    for (i = 0; i < input->frames; i++, out += 2)
  95    {
  96       float in_l = out[0];
  97       float in_r = out[1];
  98
  99       float l    = (b0 * in_l + b1 * xn1_l + b2 * xn2_l - a1 * yn1_l - a2 * yn2_l) / a0;
 100       float r    = (b0 * in_r + b1 * xn1_r + b2 * xn2_r - a1 * yn1_r - a2 * yn2_r) / a0;
 101
 102       xn2_l      = xn1_l;
 103       xn1_l      = in_l;
 104       yn2_l      = yn1_l;
 105       yn1_l      = l;
 106
 107       xn2_r      = xn1_r;
 108       xn1_r      = in_r;
 109       yn2_r      = yn1_r;
 110       yn1_r      = r;
 111
 112       out[0]     = l;
 113       out[1]     = r;
 114    }
 115
 116    iir->l.xn1 = xn1_l;
 117    iir->l.xn2 = xn2_l;
 118    iir->l.yn1 = yn1_l;
 119    iir->l.yn2 = yn2_l;
 120
 121    iir->r.xn1 = xn1_r;
 122    iir->r.xn2 = xn2_r;
 123    iir->r.yn1 = yn1_r;
 124    iir->r.yn2 = yn2_r;
 125 }
 126
 127 #define CHECK(x) if (string_is_equal(str, #x)) return x
 128 static enum IIRFilter str_to_type(const char *str)
 129 {
 130    CHECK(LPF);
 131    CHECK(HPF);
 132    CHECK(BPCSGF);
 133    CHECK(BPZPGF);
 134    CHECK(APF);
 135    CHECK(NOTCH);
 136    CHECK(RIAA_phono);
 137    CHECK(PEQ);
 138    CHECK(BBOOST);
 139    CHECK(LSH);
 140    CHECK(HSH);
 141    CHECK(RIAA_CD);
 142
 143    return LPF; /* Fallback. */
 144 }
 145
 146 static void make_poly_from_roots(
 147       const double *roots, unsigned num_roots, float *poly)
 148 {
 149    unsigned i, j;
 150
 151    poly[0] = 1;
 152    poly[1] = -roots[0];
 153    memset(poly + 2, 0, (num_roots + 1 - 2) * sizeof(*poly));
 154
 155    for (i = 1; i < num_roots; i++)
 156       for (j = num_roots; j > 0; j--)
 157          poly[j] -= poly[j - 1] * roots[i];
 158 }
 159
 160 static void iir_filter_init(struct iir_data *iir,
 161       float sample_rate, float freq, float qual, float gain, enum IIRFilter filter_type)
 162 {
 163         double omega = 2.0 * M_PI * freq / sample_rate;
 164    double cs    = cos(omega);
 165    double sn    = sin(omega);
 166    double a1pha = sn / (2.0 * qual);
 167    double A     = exp(log(10.0) * gain / 40.0);
 168    double beta  = sqrt(A + A);
 169
 170    float b0     = 0.0, b1 = 0.0, b2 = 0.0, a0 = 0.0, a1 = 0.0, a2 = 0.0;
 171
 172    /* Set up filter coefficients according to type */
 173    switch (filter_type)
 174    {
 175       case LPF:
 176          b0 =  (1.0 - cs) / 2.0;
 177          b1 =   1.0 - cs ;
 178          b2 =  (1.0 - cs) / 2.0;
 179          a0 =   1.0 + a1pha;
 180          a1 =  -2.0 * cs;
 181          a2 =   1.0 - a1pha;
 182          break;
 183       case HPF:
 184          b0 =  (1.0 + cs) / 2.0;
 185          b1 = -(1.0 + cs);
 186          b2 =  (1.0 + cs) / 2.0;
 187          a0 =   1.0 + a1pha;
 188          a1 =  -2.0 * cs;
 189          a2 =   1.0 - a1pha;
 190          break;
 191       case APF:
 192          b0 =  1.0 - a1pha;
 193          b1 = -2.0 * cs;
 194          b2 =  1.0 + a1pha;
 195          a0 =  1.0 + a1pha;
 196          a1 = -2.0 * cs;
 197          a2 =  1.0 - a1pha;
 198          break;
 199       case BPZPGF:
 200          b0 =  a1pha;
 201          b1 =  0.0;
 202          b2 = -a1pha;
 203          a0 =  1.0 + a1pha;
 204          a1 = -2.0 * cs;
 205          a2 =  1.0 - a1pha;
 206          break;
 207       case BPCSGF:
 208          b0 =  sn / 2.0;
 209          b1 =  0.0;
 210          b2 = -sn / 2.0;
 211          a0 =  1.0 + a1pha;
 212          a1 = -2.0 * cs;
 213          a2 =  1.0 - a1pha;
 214          break;
 215       case NOTCH:
 216          b0 =  1.0;
 217          b1 = -2.0 * cs;
 218          b2 =  1.0;
 219          a0 =  1.0 + a1pha;
 220          a1 = -2.0 * cs;
 221          a2 =  1.0 - a1pha;
 222          break;
 223       case RIAA_phono: /* http://www.dsprelated.com/showmessage/73300/3.php */
 224       {
 225          double y, b_re, a_re, b_im, a_im, g;
 226          float b[3] = {0.0f};
 227          float a[3] = {0.0f};
 228
 229          if ((int)sample_rate == 44100)
 230          {
 231             static const double zeros[] = {-0.2014898, 0.9233820};
 232             static const double poles[] = {0.7083149, 0.9924091};
 233             make_poly_from_roots(zeros, 2, b);
 234             make_poly_from_roots(poles, 2, a);
 235          }
 236          else if ((int)sample_rate == 48000)
 237          {
 238             static const double zeros[] = {-0.1766069, 0.9321590};
 239             static const double poles[] = {0.7396325, 0.9931330};
 240             make_poly_from_roots(zeros, 2, b);
 241             make_poly_from_roots(poles, 2, a);
 242          }
 243          else if ((int)sample_rate == 88200)
 244          {
 245             static const double zeros[] = {-0.1168735, 0.9648312};
 246             static const double poles[] = {0.8590646, 0.9964002};
 247             make_poly_from_roots(zeros, 2, b);
 248             make_poly_from_roots(poles, 2, a);
 249          }
 250          else if ((int)sample_rate == 96000)
 251          {
 252             static const double zeros[] = {-0.1141486, 0.9676817};
 253             static const double poles[] = {0.8699137, 0.9966946};
 254             make_poly_from_roots(zeros, 2, b);
 255             make_poly_from_roots(poles, 2, a);
 256          }
 257
 258          b0    = b[0];
 259          b1    = b[1];
 260          b2    = b[2];
 261          a0    = a[0];
 262          a1    = a[1];
 263          a2    = a[2];
 264
 265          /* Normalise to 0dB at 1kHz (Thanks to Glenn Davis) */
 266          y     = 2.0 * M_PI * 1000.0 / sample_rate;
 267          b_re  = b0 + b1 * cos(-y) + b2 * cos(-2.0 * y);
 268          a_re  = a0 + a1 * cos(-y) + a2 * cos(-2.0 * y);
 269          b_im  = b1 * sin(-y) + b2 * sin(-2.0 * y);
 270          a_im  = a1 * sin(-y) + a2 * sin(-2.0 * y);
 271          g     = 1.0 / sqrt((sqr(b_re) + sqr(b_im)) / (sqr(a_re) + sqr(a_im)));
 272          b0   *= g; b1 *= g; b2 *= g;
 273          break;
 274       }
 275       case PEQ:
 276          b0 =  1.0 + a1pha * A;
 277          b1 = -2.0 * cs;
 278          b2 =  1.0 - a1pha * A;
 279          a0 =  1.0 + a1pha / A;
 280          a1 = -2.0 * cs;
 281          a2 =  1.0 - a1pha / A;
 282          break;
 283       case BBOOST:
 284          beta = sqrt((A * A + 1) / 1.0 - (pow((A - 1), 2)));
 285          b0 = A * ((A + 1) - (A - 1) * cs + beta * sn);
 286          b1 = 2 * A * ((A - 1) - (A + 1) * cs);
 287          b2 = A * ((A + 1) - (A - 1) * cs - beta * sn);
 288          a0 = ((A + 1) + (A - 1) * cs + beta * sn);
 289          a1 = -2 * ((A - 1) + (A + 1) * cs);
 290          a2 = (A + 1) + (A - 1) * cs - beta * sn;
 291          break;
 292       case LSH:
 293          b0 = A * ((A + 1) - (A - 1) * cs + beta * sn);
 294          b1 = 2 * A * ((A - 1) - (A + 1) * cs);
 295          b2 = A * ((A + 1) - (A - 1) * cs - beta * sn);
 296          a0 = (A + 1) + (A - 1) * cs + beta * sn;
 297          a1 = -2 * ((A - 1) + (A + 1) * cs);
 298          a2 = (A + 1) + (A - 1) * cs - beta * sn;
 299          break;
 300       case RIAA_CD:
 301          omega = 2.0 * M_PI * 5283.0 / sample_rate;
 302          cs = cos(omega);
 303          sn = sin(omega);
 304          a1pha = sn / (2.0 * 0.4845);
 305          A = exp(log(10.0) * -9.477 / 40.0);
 306          beta = sqrt(A + A);
 307          (void)a1pha;
 308       case HSH:
 309          b0 = A * ((A + 1.0) + (A - 1.0) * cs + beta * sn);
 310          b1 = -2.0 * A * ((A - 1.0) + (A + 1.0) * cs);
 311          b2 = A * ((A + 1.0) + (A - 1.0) * cs - beta * sn);
 312          a0 = (A + 1.0) - (A - 1.0) * cs + beta * sn;
 313          a1 = 2.0 * ((A - 1.0) - (A + 1.0) * cs);
 314          a2 = (A + 1.0) - (A - 1.0) * cs - beta * sn;
 315          break;
 316       default:
 317          break;
 318    }
 319
 320    iir->b0 = b0;
 321    iir->b1 = b1;
 322    iir->b2 = b2;
 323    iir->a0 = a0;
 324    iir->a1 = a1;
 325    iir->a2 = a2;
 326 }
 327
 328 static void *iir_init(const struct dspfilter_info *info,
 329       const struct dspfilter_config *config, void *userdata)
 330 {
 331    float freq, qual, gain;
 332    enum IIRFilter filter  = LPF;
 333    char           *type   = NULL;
 334    struct iir_data *iir   = (struct iir_data*)calloc(1, sizeof(*iir));
 335    if (!iir)
 336       return NULL;
 337
 338    config->get_float(userdata, "frequency", &freq, 1024.0f);
 339    config->get_float(userdata, "quality", &qual, 0.707f);
 340    config->get_float(userdata, "gain", &gain, 0.0f);
 341
 342    config->get_string(userdata, "type", &type, "LPF");
 343
 344    filter = str_to_type(type);
 345    config->free(type);
 346
 347    iir_filter_init(iir, info->input_rate, freq, qual, gain, filter);
 348    return iir;
 349 }
 350
 351 static const struct dspfilter_implementation iir_plug = {
 352    iir_init,
 353    iir_process,
 354    iir_free,
 355
 356    DSPFILTER_API_VERSION,
 357    "IIR",
 358    "iir",
 359 };
 360
 361 #ifdef HAVE_FILTERS_BUILTIN
 362 #define dspfilter_get_implementation iir_dspfilter_get_implementation
 363 #endif
 364
 365 const struct dspfilter_implementation *dspfilter_get_implementation(dspfilter_simd_mask_t mask)
 366 {
 367    return &iir_plug;
 368 }
 369
 370 #undef dspfilter_get_implementation