/* 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], a[3]; 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) { (void)mask; return &iir_plug; } #undef dspfilter_get_implementation