X-Git-Url: https://notaz.gp2x.de/cgi-bin/gitweb.cgi?a=blobdiff_plain;f=deps%2Flibretro-common%2Faudio%2Fdsp_filters%2Fiir.c;fp=deps%2Flibretro-common%2Faudio%2Fdsp_filters%2Fiir.c;h=232edc15d720b4a9b087771e8f084469ff48abe5;hb=3719602cbe883fb394a71353e20a10a4a306e814;hp=0000000000000000000000000000000000000000;hpb=8659d7fd2cdb11f63724ead0997f47f4c694f8c2;p=pcsx_rearmed.git diff --git a/deps/libretro-common/audio/dsp_filters/iir.c b/deps/libretro-common/audio/dsp_filters/iir.c new file mode 100644 index 00000000..232edc15 --- /dev/null +++ b/deps/libretro-common/audio/dsp_filters/iir.c @@ -0,0 +1,370 @@ +/* Copyright (C) 2010-2020 The RetroArch team + * + * --------------------------------------------------------------------------------------- + * The following license statement only applies to this file (iir.c). + * --------------------------------------------------------------------------------------- + * + * Permission is hereby granted, free of charge, + * to any person obtaining a copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, + * and to permit persons to whom the Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, + * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. + * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, + * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include +#include +#include + +#include +#include +#include + +#define sqr(a) ((a) * (a)) + +/* filter types */ +enum IIRFilter +{ + LPF, /* low pass filter */ + HPF, /* High pass filter */ + BPCSGF, /* band pass filter 1 */ + BPZPGF, /* band pass filter 2 */ + APF, /* Allpass filter*/ + NOTCH, /* Notch Filter */ + RIAA_phono, /* RIAA record/tape deemphasis */ + PEQ, /* Peaking band EQ filter */ + BBOOST, /* Bassboost filter */ + LSH, /* Low shelf filter */ + HSH, /* High shelf filter */ + RIAA_CD /* CD de-emphasis */ +}; + +struct iir_data +{ + float b0, b1, b2; + float a0, a1, a2; + + struct + { + float xn1, xn2; + float yn1, yn2; + } l, r; +}; + +static void iir_free(void *data) +{ + free(data); +} + +static void iir_process(void *data, struct dspfilter_output *output, + const struct dspfilter_input *input) +{ + unsigned i; + struct iir_data *iir = (struct iir_data*)data; + float *out = output->samples; + + float b0 = iir->b0; + float b1 = iir->b1; + float b2 = iir->b2; + float a0 = iir->a0; + float a1 = iir->a1; + float a2 = iir->a2; + + float xn1_l = iir->l.xn1; + float xn2_l = iir->l.xn2; + float yn1_l = iir->l.yn1; + float yn2_l = iir->l.yn2; + + float xn1_r = iir->r.xn1; + float xn2_r = iir->r.xn2; + float yn1_r = iir->r.yn1; + float yn2_r = iir->r.yn2; + + output->samples = input->samples; + output->frames = input->frames; + + for (i = 0; i < input->frames; i++, out += 2) + { + float in_l = out[0]; + float in_r = out[1]; + + float l = (b0 * in_l + b1 * xn1_l + b2 * xn2_l - a1 * yn1_l - a2 * yn2_l) / a0; + float r = (b0 * in_r + b1 * xn1_r + b2 * xn2_r - a1 * yn1_r - a2 * yn2_r) / a0; + + xn2_l = xn1_l; + xn1_l = in_l; + yn2_l = yn1_l; + yn1_l = l; + + xn2_r = xn1_r; + xn1_r = in_r; + yn2_r = yn1_r; + yn1_r = r; + + out[0] = l; + out[1] = r; + } + + iir->l.xn1 = xn1_l; + iir->l.xn2 = xn2_l; + iir->l.yn1 = yn1_l; + iir->l.yn2 = yn2_l; + + iir->r.xn1 = xn1_r; + iir->r.xn2 = xn2_r; + iir->r.yn1 = yn1_r; + iir->r.yn2 = yn2_r; +} + +#define CHECK(x) if (string_is_equal(str, #x)) return x +static enum IIRFilter str_to_type(const char *str) +{ + CHECK(LPF); + CHECK(HPF); + CHECK(BPCSGF); + CHECK(BPZPGF); + CHECK(APF); + CHECK(NOTCH); + CHECK(RIAA_phono); + CHECK(PEQ); + CHECK(BBOOST); + CHECK(LSH); + CHECK(HSH); + CHECK(RIAA_CD); + + return LPF; /* Fallback. */ +} + +static void make_poly_from_roots( + const double *roots, unsigned num_roots, float *poly) +{ + unsigned i, j; + + poly[0] = 1; + poly[1] = -roots[0]; + memset(poly + 2, 0, (num_roots + 1 - 2) * sizeof(*poly)); + + for (i = 1; i < num_roots; i++) + for (j = num_roots; j > 0; j--) + poly[j] -= poly[j - 1] * roots[i]; +} + +static void iir_filter_init(struct iir_data *iir, + float sample_rate, float freq, float qual, float gain, enum IIRFilter filter_type) +{ + double omega = 2.0 * M_PI * freq / sample_rate; + double cs = cos(omega); + double sn = sin(omega); + double a1pha = sn / (2.0 * qual); + double A = exp(log(10.0) * gain / 40.0); + double beta = sqrt(A + A); + + float b0 = 0.0, b1 = 0.0, b2 = 0.0, a0 = 0.0, a1 = 0.0, a2 = 0.0; + + /* Set up filter coefficients according to type */ + switch (filter_type) + { + case LPF: + b0 = (1.0 - cs) / 2.0; + b1 = 1.0 - cs ; + b2 = (1.0 - cs) / 2.0; + a0 = 1.0 + a1pha; + a1 = -2.0 * cs; + a2 = 1.0 - a1pha; + break; + case HPF: + b0 = (1.0 + cs) / 2.0; + b1 = -(1.0 + cs); + b2 = (1.0 + cs) / 2.0; + a0 = 1.0 + a1pha; + a1 = -2.0 * cs; + a2 = 1.0 - a1pha; + break; + case APF: + b0 = 1.0 - a1pha; + b1 = -2.0 * cs; + b2 = 1.0 + a1pha; + a0 = 1.0 + a1pha; + a1 = -2.0 * cs; + a2 = 1.0 - a1pha; + break; + case BPZPGF: + b0 = a1pha; + b1 = 0.0; + b2 = -a1pha; + a0 = 1.0 + a1pha; + a1 = -2.0 * cs; + a2 = 1.0 - a1pha; + break; + case BPCSGF: + b0 = sn / 2.0; + b1 = 0.0; + b2 = -sn / 2.0; + a0 = 1.0 + a1pha; + a1 = -2.0 * cs; + a2 = 1.0 - a1pha; + break; + case NOTCH: + b0 = 1.0; + b1 = -2.0 * cs; + b2 = 1.0; + a0 = 1.0 + a1pha; + a1 = -2.0 * cs; + a2 = 1.0 - a1pha; + break; + case RIAA_phono: /* http://www.dsprelated.com/showmessage/73300/3.php */ + { + double y, b_re, a_re, b_im, a_im, g; + float b[3] = {0.0f}; + float a[3] = {0.0f}; + + if ((int)sample_rate == 44100) + { + static const double zeros[] = {-0.2014898, 0.9233820}; + static const double poles[] = {0.7083149, 0.9924091}; + make_poly_from_roots(zeros, 2, b); + make_poly_from_roots(poles, 2, a); + } + else if ((int)sample_rate == 48000) + { + static const double zeros[] = {-0.1766069, 0.9321590}; + static const double poles[] = {0.7396325, 0.9931330}; + make_poly_from_roots(zeros, 2, b); + make_poly_from_roots(poles, 2, a); + } + else if ((int)sample_rate == 88200) + { + static const double zeros[] = {-0.1168735, 0.9648312}; + static const double poles[] = {0.8590646, 0.9964002}; + make_poly_from_roots(zeros, 2, b); + make_poly_from_roots(poles, 2, a); + } + else if ((int)sample_rate == 96000) + { + static const double zeros[] = {-0.1141486, 0.9676817}; + static const double poles[] = {0.8699137, 0.9966946}; + make_poly_from_roots(zeros, 2, b); + make_poly_from_roots(poles, 2, a); + } + + b0 = b[0]; + b1 = b[1]; + b2 = b[2]; + a0 = a[0]; + a1 = a[1]; + a2 = a[2]; + + /* Normalise to 0dB at 1kHz (Thanks to Glenn Davis) */ + y = 2.0 * M_PI * 1000.0 / sample_rate; + b_re = b0 + b1 * cos(-y) + b2 * cos(-2.0 * y); + a_re = a0 + a1 * cos(-y) + a2 * cos(-2.0 * y); + b_im = b1 * sin(-y) + b2 * sin(-2.0 * y); + a_im = a1 * sin(-y) + a2 * sin(-2.0 * y); + g = 1.0 / sqrt((sqr(b_re) + sqr(b_im)) / (sqr(a_re) + sqr(a_im))); + b0 *= g; b1 *= g; b2 *= g; + break; + } + case PEQ: + b0 = 1.0 + a1pha * A; + b1 = -2.0 * cs; + b2 = 1.0 - a1pha * A; + a0 = 1.0 + a1pha / A; + a1 = -2.0 * cs; + a2 = 1.0 - a1pha / A; + break; + case BBOOST: + beta = sqrt((A * A + 1) / 1.0 - (pow((A - 1), 2))); + b0 = A * ((A + 1) - (A - 1) * cs + beta * sn); + b1 = 2 * A * ((A - 1) - (A + 1) * cs); + b2 = A * ((A + 1) - (A - 1) * cs - beta * sn); + a0 = ((A + 1) + (A - 1) * cs + beta * sn); + a1 = -2 * ((A - 1) + (A + 1) * cs); + a2 = (A + 1) + (A - 1) * cs - beta * sn; + break; + case LSH: + b0 = A * ((A + 1) - (A - 1) * cs + beta * sn); + b1 = 2 * A * ((A - 1) - (A + 1) * cs); + b2 = A * ((A + 1) - (A - 1) * cs - beta * sn); + a0 = (A + 1) + (A - 1) * cs + beta * sn; + a1 = -2 * ((A - 1) + (A + 1) * cs); + a2 = (A + 1) + (A - 1) * cs - beta * sn; + break; + case RIAA_CD: + omega = 2.0 * M_PI * 5283.0 / sample_rate; + cs = cos(omega); + sn = sin(omega); + a1pha = sn / (2.0 * 0.4845); + A = exp(log(10.0) * -9.477 / 40.0); + beta = sqrt(A + A); + (void)a1pha; + case HSH: + b0 = A * ((A + 1.0) + (A - 1.0) * cs + beta * sn); + b1 = -2.0 * A * ((A - 1.0) + (A + 1.0) * cs); + b2 = A * ((A + 1.0) + (A - 1.0) * cs - beta * sn); + a0 = (A + 1.0) - (A - 1.0) * cs + beta * sn; + a1 = 2.0 * ((A - 1.0) - (A + 1.0) * cs); + a2 = (A + 1.0) - (A - 1.0) * cs - beta * sn; + break; + default: + break; + } + + iir->b0 = b0; + iir->b1 = b1; + iir->b2 = b2; + iir->a0 = a0; + iir->a1 = a1; + iir->a2 = a2; +} + +static void *iir_init(const struct dspfilter_info *info, + const struct dspfilter_config *config, void *userdata) +{ + float freq, qual, gain; + enum IIRFilter filter = LPF; + char *type = NULL; + struct iir_data *iir = (struct iir_data*)calloc(1, sizeof(*iir)); + if (!iir) + return NULL; + + config->get_float(userdata, "frequency", &freq, 1024.0f); + config->get_float(userdata, "quality", &qual, 0.707f); + config->get_float(userdata, "gain", &gain, 0.0f); + + config->get_string(userdata, "type", &type, "LPF"); + + filter = str_to_type(type); + config->free(type); + + iir_filter_init(iir, info->input_rate, freq, qual, gain, filter); + return iir; +} + +static const struct dspfilter_implementation iir_plug = { + iir_init, + iir_process, + iir_free, + + DSPFILTER_API_VERSION, + "IIR", + "iir", +}; + +#ifdef HAVE_FILTERS_BUILTIN +#define dspfilter_get_implementation iir_dspfilter_get_implementation +#endif + +const struct dspfilter_implementation *dspfilter_get_implementation(dspfilter_simd_mask_t mask) +{ + return &iir_plug; +} + +#undef dspfilter_get_implementation