| 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 |