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