Commit | Line | Data |
---|---|---|
3719602c PC |
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 |