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 (eq.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 <stdio.h> | |
24 | #include <stdlib.h> | |
25 | #include <string.h> | |
26 | ||
27 | #include <retro_inline.h> | |
28 | #include <retro_miscellaneous.h> | |
29 | #include <filters.h> | |
30 | #include <libretro_dspfilter.h> | |
31 | ||
32 | #include "fft/fft.c" | |
33 | ||
34 | struct eq_data | |
35 | { | |
36 | fft_t *fft; | |
37 | float *save; | |
38 | float *block; | |
39 | fft_complex_t *filter; | |
40 | fft_complex_t *fftblock; | |
41 | float buffer[8 * 1024]; | |
42 | unsigned block_size; | |
43 | unsigned block_ptr; | |
44 | }; | |
45 | ||
46 | struct eq_gain | |
47 | { | |
48 | float freq; | |
49 | float gain; /* Linear. */ | |
50 | }; | |
51 | ||
52 | static void eq_free(void *data) | |
53 | { | |
54 | struct eq_data *eq = (struct eq_data*)data; | |
55 | if (!eq) | |
56 | return; | |
57 | ||
58 | fft_free(eq->fft); | |
59 | free(eq->save); | |
60 | free(eq->block); | |
61 | free(eq->fftblock); | |
62 | free(eq->filter); | |
63 | free(eq); | |
64 | } | |
65 | ||
66 | static void eq_process(void *data, struct dspfilter_output *output, | |
67 | const struct dspfilter_input *input) | |
68 | { | |
69 | float *out; | |
70 | const float *in; | |
71 | unsigned input_frames; | |
72 | struct eq_data *eq = (struct eq_data*)data; | |
73 | ||
74 | output->samples = eq->buffer; | |
75 | output->frames = 0; | |
76 | ||
77 | out = eq->buffer; | |
78 | in = input->samples; | |
79 | input_frames = input->frames; | |
80 | ||
81 | while (input_frames) | |
82 | { | |
83 | unsigned write_avail = eq->block_size - eq->block_ptr; | |
84 | ||
85 | if (input_frames < write_avail) | |
86 | write_avail = input_frames; | |
87 | ||
88 | memcpy(eq->block + eq->block_ptr * 2, in, write_avail * 2 * sizeof(float)); | |
89 | ||
90 | in += write_avail * 2; | |
91 | input_frames -= write_avail; | |
92 | eq->block_ptr += write_avail; | |
93 | ||
94 | /* Convolve a new block. */ | |
95 | if (eq->block_ptr == eq->block_size) | |
96 | { | |
97 | unsigned i, c; | |
98 | ||
99 | for (c = 0; c < 2; c++) | |
100 | { | |
101 | fft_process_forward(eq->fft, eq->fftblock, eq->block + c, 2); | |
102 | for (i = 0; i < 2 * eq->block_size; i++) | |
103 | eq->fftblock[i] = fft_complex_mul(eq->fftblock[i], eq->filter[i]); | |
104 | fft_process_inverse(eq->fft, out + c, eq->fftblock, 2); | |
105 | } | |
106 | ||
107 | /* Overlap add method, so add in saved block now. */ | |
108 | for (i = 0; i < 2 * eq->block_size; i++) | |
109 | out[i] += eq->save[i]; | |
110 | ||
111 | /* Save block for later. */ | |
112 | memcpy(eq->save, out + 2 * eq->block_size, 2 * eq->block_size * sizeof(float)); | |
113 | ||
114 | out += eq->block_size * 2; | |
115 | output->frames += eq->block_size; | |
116 | eq->block_ptr = 0; | |
117 | } | |
118 | } | |
119 | } | |
120 | ||
121 | static int gains_cmp(const void *a_, const void *b_) | |
122 | { | |
123 | const struct eq_gain *a = (const struct eq_gain*)a_; | |
124 | const struct eq_gain *b = (const struct eq_gain*)b_; | |
125 | if (a->freq < b->freq) | |
126 | return -1; | |
127 | if (a->freq > b->freq) | |
128 | return 1; | |
129 | return 0; | |
130 | } | |
131 | ||
132 | static void generate_response(fft_complex_t *response, | |
133 | const struct eq_gain *gains, unsigned num_gains, unsigned samples) | |
134 | { | |
135 | unsigned i; | |
136 | ||
137 | float start_freq = 0.0f; | |
138 | float start_gain = 1.0f; | |
139 | ||
140 | float end_freq = 1.0f; | |
141 | float end_gain = 1.0f; | |
142 | ||
143 | if (num_gains) | |
144 | { | |
145 | end_freq = gains->freq; | |
146 | end_gain = gains->gain; | |
147 | num_gains--; | |
148 | gains++; | |
149 | } | |
150 | ||
151 | /* Create a response by linear interpolation between | |
152 | * known frequency sample points. */ | |
153 | for (i = 0; i <= samples; i++) | |
154 | { | |
155 | float gain; | |
156 | float lerp = 0.5f; | |
157 | float freq = (float)i / samples; | |
158 | ||
159 | while (freq >= end_freq) | |
160 | { | |
161 | if (num_gains) | |
162 | { | |
163 | start_freq = end_freq; | |
164 | start_gain = end_gain; | |
165 | end_freq = gains->freq; | |
166 | end_gain = gains->gain; | |
167 | ||
168 | gains++; | |
169 | num_gains--; | |
170 | } | |
171 | else | |
172 | { | |
173 | start_freq = end_freq; | |
174 | start_gain = end_gain; | |
175 | end_freq = 1.0f; | |
176 | end_gain = 1.0f; | |
177 | break; | |
178 | } | |
179 | } | |
180 | ||
181 | /* Edge case where i == samples. */ | |
182 | if (end_freq > start_freq) | |
183 | lerp = (freq - start_freq) / (end_freq - start_freq); | |
184 | gain = (1.0f - lerp) * start_gain + lerp * end_gain; | |
185 | ||
186 | response[i].real = gain; | |
187 | response[i].imag = 0.0f; | |
188 | response[2 * samples - i].real = gain; | |
189 | response[2 * samples - i].imag = 0.0f; | |
190 | } | |
191 | } | |
192 | ||
193 | static void create_filter(struct eq_data *eq, unsigned size_log2, | |
194 | struct eq_gain *gains, unsigned num_gains, double beta, const char *filter_path) | |
195 | { | |
196 | int i; | |
197 | int half_block_size = eq->block_size >> 1; | |
198 | double window_mod = 1.0 / kaiser_window_function(0.0, beta); | |
199 | fft_t *fft = fft_new(size_log2); | |
200 | float *time_filter = (float*)calloc(eq->block_size * 2 + 1, sizeof(*time_filter)); | |
201 | if (!fft || !time_filter) | |
202 | goto end; | |
203 | ||
204 | /* Make sure bands are in correct order. */ | |
205 | qsort(gains, num_gains, sizeof(*gains), gains_cmp); | |
206 | ||
207 | /* Compute desired filter response. */ | |
208 | generate_response(eq->filter, gains, num_gains, half_block_size); | |
209 | ||
210 | /* Get equivalent time-domain filter. */ | |
211 | fft_process_inverse(fft, time_filter, eq->filter, 1); | |
212 | ||
213 | /* ifftshift() to create the correct linear phase filter. | |
214 | * The filter response was designed with zero phase, which | |
215 | * won't work unless we compensate | |
216 | * for the repeating property of the FFT here | |
217 | * by flipping left and right blocks. */ | |
218 | for (i = 0; i < half_block_size; i++) | |
219 | { | |
220 | float tmp = time_filter[i + half_block_size]; | |
221 | time_filter[i + half_block_size] = time_filter[i]; | |
222 | time_filter[i] = tmp; | |
223 | } | |
224 | ||
225 | /* Apply a window to smooth out the frequency repsonse. */ | |
226 | for (i = 0; i < (int)eq->block_size; i++) | |
227 | { | |
228 | /* Kaiser window. */ | |
229 | double phase = (double)i / eq->block_size; | |
230 | phase = 2.0 * (phase - 0.5); | |
231 | time_filter[i] *= window_mod * kaiser_window_function(phase, beta); | |
232 | } | |
233 | ||
234 | #ifdef DEBUG | |
235 | /* Debugging. */ | |
236 | if (filter_path) | |
237 | { | |
238 | FILE *file = fopen(filter_path, "w"); | |
239 | if (file) | |
240 | { | |
241 | for (i = 0; i < (int)eq->block_size - 1; i++) | |
242 | fprintf(file, "%.8f\n", time_filter[i + 1]); | |
243 | fclose(file); | |
244 | } | |
245 | } | |
246 | #endif | |
247 | ||
248 | /* Padded FFT to create our FFT filter. | |
249 | * Make our even-length filter odd by discarding the first coefficient. | |
250 | * For some interesting reason, this allows us to design an odd-length linear phase filter. | |
251 | */ | |
252 | fft_process_forward(eq->fft, eq->filter, time_filter + 1, 1); | |
253 | ||
254 | end: | |
255 | fft_free(fft); | |
256 | free(time_filter); | |
257 | } | |
258 | ||
259 | static void *eq_init(const struct dspfilter_info *info, | |
260 | const struct dspfilter_config *config, void *userdata) | |
261 | { | |
262 | int size_log2; | |
263 | float beta; | |
264 | float *frequencies, *gain; | |
265 | unsigned num_freq, num_gain, i, size; | |
266 | struct eq_gain *gains = NULL; | |
267 | char *filter_path = NULL; | |
268 | const float default_freq[] = { 0.0f, info->input_rate }; | |
269 | const float default_gain[] = { 0.0f, 0.0f }; | |
270 | struct eq_data *eq = (struct eq_data*)calloc(1, sizeof(*eq)); | |
271 | if (!eq) | |
272 | return NULL; | |
273 | ||
274 | config->get_float(userdata, "window_beta", &beta, 4.0f); | |
275 | ||
276 | config->get_int(userdata, "block_size_log2", &size_log2, 8); | |
277 | size = 1 << size_log2; | |
278 | ||
279 | config->get_float_array(userdata, "frequencies", &frequencies, &num_freq, default_freq, 2); | |
280 | config->get_float_array(userdata, "gains", &gain, &num_gain, default_gain, 2); | |
281 | ||
282 | if (!config->get_string(userdata, "impulse_response_output", &filter_path, "")) | |
283 | { | |
284 | config->free(filter_path); | |
285 | filter_path = NULL; | |
286 | } | |
287 | ||
288 | num_gain = num_freq = MIN(num_gain, num_freq); | |
289 | ||
290 | if (!(gains = (struct eq_gain*)calloc(num_gain, sizeof(*gains)))) | |
291 | goto error; | |
292 | ||
293 | for (i = 0; i < num_gain; i++) | |
294 | { | |
295 | gains[i].freq = frequencies[i] / (0.5f * info->input_rate); | |
296 | gains[i].gain = pow(10.0, gain[i] / 20.0); | |
297 | } | |
298 | config->free(frequencies); | |
299 | config->free(gain); | |
300 | ||
301 | eq->block_size = size; | |
302 | ||
303 | eq->save = (float*)calloc( size, 2 * sizeof(*eq->save)); | |
304 | eq->block = (float*)calloc(2 * size, 2 * sizeof(*eq->block)); | |
305 | eq->fftblock = (fft_complex_t*)calloc(2 * size, sizeof(*eq->fftblock)); | |
306 | eq->filter = (fft_complex_t*)calloc(2 * size, sizeof(*eq->filter)); | |
307 | ||
308 | /* Use an FFT which is twice the block size with zero-padding | |
309 | * to make circular convolution => proper convolution. | |
310 | */ | |
311 | eq->fft = fft_new(size_log2 + 1); | |
312 | ||
313 | if (!eq->fft || !eq->fftblock || !eq->save || !eq->block || !eq->filter) | |
314 | goto error; | |
315 | ||
316 | create_filter(eq, size_log2, gains, num_gain, beta, filter_path); | |
317 | config->free(filter_path); | |
318 | filter_path = NULL; | |
319 | ||
320 | free(gains); | |
321 | return eq; | |
322 | ||
323 | error: | |
324 | free(gains); | |
325 | eq_free(eq); | |
326 | return NULL; | |
327 | } | |
328 | ||
329 | static const struct dspfilter_implementation eq_plug = { | |
330 | eq_init, | |
331 | eq_process, | |
332 | eq_free, | |
333 | ||
334 | DSPFILTER_API_VERSION, | |
335 | "Linear-Phase FFT Equalizer", | |
336 | "eq", | |
337 | }; | |
338 | ||
339 | #ifdef HAVE_FILTERS_BUILTIN | |
340 | #define dspfilter_get_implementation eq_dspfilter_get_implementation | |
341 | #endif | |
342 | ||
343 | const struct dspfilter_implementation *dspfilter_get_implementation(dspfilter_simd_mask_t mask) | |
344 | { | |
345 | return &eq_plug; | |
346 | } | |
347 | ||
348 | #undef dspfilter_get_implementation |