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