diff --git a/audio/dsp_filter.c b/audio/dsp_filter.c index 939c6ce455..48ecaf2690 100644 --- a/audio/dsp_filter.c +++ b/audio/dsp_filter.c @@ -226,6 +226,7 @@ extern const struct dspfilter_implementation *iir_dspfilter_get_implementation(d extern const struct dspfilter_implementation *echo_dspfilter_get_implementation(dspfilter_simd_mask_t mask); extern const struct dspfilter_implementation *phaser_dspfilter_get_implementation(dspfilter_simd_mask_t mask); extern const struct dspfilter_implementation *wahwah_dspfilter_get_implementation(dspfilter_simd_mask_t mask); +extern const struct dspfilter_implementation *eq_dspfilter_get_implementation(dspfilter_simd_mask_t mask); static const dspfilter_get_implementation_t dsp_plugs_builtin[] = { panning_dspfilter_get_implementation, @@ -233,6 +234,7 @@ static const dspfilter_get_implementation_t dsp_plugs_builtin[] = { echo_dspfilter_get_implementation, phaser_dspfilter_get_implementation, wahwah_dspfilter_get_implementation, + eq_dspfilter_get_implementation, }; static bool append_plugs(rarch_dsp_filter_t *dsp) diff --git a/audio/filters/eq.c b/audio/filters/eq.c new file mode 100644 index 0000000000..0b4a5d24f7 --- /dev/null +++ b/audio/filters/eq.c @@ -0,0 +1,362 @@ +/* RetroArch - A frontend for libretro. + * Copyright (C) 2010-2014 - Hans-Kristian Arntzen + * + * RetroArch is free software: you can redistribute it and/or modify it under the terms + * of the GNU General Public License as published by the Free Software Found- + * ation, either version 3 of the License, or (at your option) any later version. + * + * RetroArch is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; + * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with RetroArch. + * If not, see . + */ + +#include "dspfilter.h" +#include +#include +#include + +#include "fft/fft.c" + +#ifndef M_PI +#define M_PI 3.1415926535897932384626433832795 +#endif + +#ifndef min +#define min(a, b) ((a) < (b) ? (a) : (b)) +#endif + +struct eq_data +{ + fft_t *fft; + float buffer[8 * 1024]; + + float *save; + float *block; + fft_complex_t *filter; + fft_complex_t *fftblock; + unsigned block_size; + unsigned block_ptr; +}; + +struct eq_gain +{ + float freq; + float gain; // Linear. +}; + +static void eq_free(void *data) +{ + struct eq_data *eq = (struct eq_data*)data; + if (!eq) + return; + + fft_free(eq->fft); + free(eq->save); + free(eq->block); + free(eq->fftblock); + free(eq->filter); + free(eq); +} + +static void eq_process(void *data, struct dspfilter_output *output, + const struct dspfilter_input *input) +{ + struct eq_data *eq = (struct eq_data*)data; + + output->samples = eq->buffer; + output->frames = 0; + + float *out = eq->buffer; + const float *in = input->samples; + unsigned input_frames = input->frames; + + while (input_frames) + { + unsigned write_avail = eq->block_size - eq->block_ptr; + if (input_frames < write_avail) + write_avail = input_frames; + + memcpy(eq->block + eq->block_ptr * 2, in, write_avail * 2 * sizeof(float)); + + in += write_avail * 2; + input_frames -= write_avail; + eq->block_ptr += write_avail; + + // Convolve a new block. + if (eq->block_ptr == eq->block_size) + { + unsigned i, c; + + for (c = 0; c < 2; c++) + { + fft_process_forward(eq->fft, eq->fftblock, eq->block + c, 2); + for (i = 0; i < 2 * eq->block_size; i++) + eq->fftblock[i] = fft_complex_mul(eq->fftblock[i], eq->filter[i]); + fft_process_inverse(eq->fft, out + c, eq->fftblock, 2); + } + + // Overlap add method, so add in saved block now. + for (i = 0; i < 2 * eq->block_size; i++) + out[i] += eq->save[i]; + + // Save block for later. + memcpy(eq->save, out + 2 * eq->block_size, 2 * eq->block_size * sizeof(float)); + + out += eq->block_size * 2; + output->frames += eq->block_size; + eq->block_ptr = 0; + } + } +} + +static int gains_cmp(const void *a_, const void *b_) +{ + const struct eq_gain *a = (const struct eq_gain*)a_; + const struct eq_gain *b = (const struct eq_gain*)b_; + if (a->freq < b->freq) + return -1; + else if (a->freq > b->freq) + return 1; + else + return 0; +} + +static void generate_response(fft_complex_t *response, + const struct eq_gain *gains, unsigned num_gains, unsigned samples) +{ + unsigned i; + + float start_freq = 0.0f; + float start_gain = 1.0f; + + float end_freq = 1.0f; + float end_gain = 1.0f; + + if (num_gains) + { + end_freq = gains->freq; + end_gain = gains->gain; + num_gains--; + gains++; + } + + // Create a response by linear interpolation between known frequency sample points. + for (i = 0; i <= samples; i++) + { + float freq = (float)i / samples; + + while (freq >= end_freq) + { + if (num_gains) + { + start_freq = end_freq; + start_gain = end_gain; + end_freq = gains->freq; + end_gain = gains->gain; + + gains++; + num_gains--; + } + else + { + start_freq = end_freq; + start_gain = end_gain; + end_freq = 1.0f; + end_gain = 1.0f; + break; + } + } + + float lerp = 0.5f; + // Edge case where i == samples. + if (end_freq > start_freq) + lerp = (freq - start_freq) / (end_freq - start_freq); + float gain = (1.0f - lerp) * start_gain + lerp * end_gain; + + response[i].real = gain; + response[i].imag = 0.0f; + response[2 * samples - i].real = gain; + response[2 * samples - i].imag = 0.0f; + } +} + +// Modified Bessel function of first order. +// Check Wiki for mathematical definition ... +static inline double kaiser_besseli0(double x) +{ + unsigned i; + double sum = 0.0; + + double factorial = 1.0; + double factorial_mult = 0.0; + double x_pow = 1.0; + double two_div_pow = 1.0; + double x_sqr = x * x; + + // Approximate. This is an infinite sum. + // Luckily, it converges rather fast. + for (i = 0; i < 18; i++) + { + sum += x_pow * two_div_pow / (factorial * factorial); + + factorial_mult += 1.0; + x_pow *= x_sqr; + two_div_pow *= 0.25; + factorial *= factorial_mult; + } + + return sum; +} + +static inline double kaiser_window(double index, double beta) +{ + return kaiser_besseli0(beta * sqrt(1 - index * index)); +} + +static void create_filter(struct eq_data *eq, unsigned size_log2, + struct eq_gain *gains, unsigned num_gains, double beta) +{ + int i; + int half_block_size = eq->block_size >> 1; + double window_mod = 1.0 / kaiser_window(0.0, beta); + + fft_t *fft = fft_new(size_log2); + float *time_filter = (float*)calloc(eq->block_size * 2, sizeof(*time_filter)); + if (!fft || !time_filter) + goto end; + + // Make sure bands are in correct order. + qsort(gains, num_gains, sizeof(*gains), gains_cmp); + + // Compute desired filter response. + generate_response(eq->filter, gains, num_gains, half_block_size); + + // Get equivalent time-domain filter. + fft_process_inverse(fft, time_filter, eq->filter, 1); + + // ifftshift() to create the correct linear phase filter. + // The filter response was designed with zero phase, which won't work unless we compensate + // for the repeating property of the FFT here by flipping left and right blocks. + for (i = 0; i < half_block_size; i++) + { + float tmp = time_filter[i + half_block_size]; + time_filter[i + half_block_size] = time_filter[i]; + time_filter[i] = tmp; + } + + // Apply a window to smooth out the frequency repsonse. + for (i = 0; i < (int)eq->block_size; i++) + { + // Simple cosine window. + double phase = (double)i / (eq->block_size - 1); + phase = 2.0 * (phase - 0.5); + time_filter[i] *= window_mod * kaiser_window(phase, beta); + } + + // Debugging. +#if 0 + FILE *file = fopen("/tmp/test.txt", "w"); + if (file) + { + for (i = 0; i < (int)eq->block_size; i++) + fprintf(file, "%.6f\n", time_filter[i]); + fclose(file); + } +#endif + + // Padded FFT to create our FFT filter. + fft_process_forward(eq->fft, eq->filter, time_filter, 1); + +end: + fft_free(fft); + free(time_filter); +} + +static void *eq_init(const struct dspfilter_info *info, + const struct dspfilter_config *config, void *userdata) +{ + unsigned i; + struct eq_data *eq = (struct eq_data*)calloc(1, sizeof(*eq)); + if (!eq) + return NULL; + + const float default_freq[] = { 0.0f, info->input_rate }; + const float default_gain[] = { 0.0f, 0.0f }; + + float beta; + config->get_float(userdata, "window_beta", &beta, 4.0f); + + int size_log2; + config->get_int(userdata, "block_size_log2", &size_log2, 8); + unsigned size = 1 << size_log2; + + struct eq_gain *gains = NULL; + float *frequencies, *gain; + unsigned num_freq, num_gain; + config->get_float_array(userdata, "frequencies", &frequencies, &num_freq, default_freq, 2); + config->get_float_array(userdata, "gains", &gain, &num_gain, default_gain, 2); + + num_gain = num_freq = min(num_gain, num_freq); + + gains = (struct eq_gain*)calloc(num_gain, sizeof(*gains)); + if (!gains) + goto error; + + for (i = 0; i < num_gain; i++) + { + gains[i].freq = frequencies[i] / (0.5f * info->input_rate); + gains[i].gain = pow(10.0, gain[i] / 20.0); + } + config->free(frequencies); + config->free(gain); + + eq->block_size = size; + + eq->save = (float*)calloc( size, 2 * sizeof(*eq->save)); + eq->block = (float*)calloc(2 * size, 2 * sizeof(*eq->block)); + eq->fftblock = (fft_complex_t*)calloc(2 * size, sizeof(*eq->fftblock)); + eq->filter = (fft_complex_t*)calloc(2 * size, sizeof(*eq->filter)); + + // Use an FFT which is twice the block size with zero-padding + // to make circular convolution => proper convolution. + eq->fft = fft_new(size_log2 + 1); + + if (!eq->fft || !eq->fftblock || !eq->save || !eq->block || !eq->filter) + goto error; + + create_filter(eq, size_log2, gains, num_gain, beta); + + free(gains); + return eq; + +error: + free(gains); + eq_free(eq); + return NULL; +} + +static const struct dspfilter_implementation eq_plug = { + eq_init, + eq_process, + eq_free, + + DSPFILTER_API_VERSION, + "Linear-Phase FFT Equalizer", + "eq", +}; + +#ifdef HAVE_FILTERS_BUILTIN +#define dspfilter_get_implementation eq_dspfilter_get_implementation +#endif + +const struct dspfilter_implementation *dspfilter_get_implementation(dspfilter_simd_mask_t mask) +{ + (void)mask; + return &eq_plug; +} + +#undef dspfilter_get_implementation + diff --git a/audio/filters/fft/fft.c b/audio/filters/fft/fft.c new file mode 100644 index 0000000000..aac3427ee9 --- /dev/null +++ b/audio/filters/fft/fft.c @@ -0,0 +1,196 @@ +/* RetroArch - A frontend for libretro. + * Copyright (C) 2010-2014 - Hans-Kristian Arntzen + * + * RetroArch is free software: you can redistribute it and/or modify it under the terms + * of the GNU General Public License as published by the Free Software Found- + * ation, either version 3 of the License, or (at your option) any later version. + * + * RetroArch is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; + * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with RetroArch. + * If not, see . + */ + +#include "fft.h" +#include +#include + +#ifndef M_PI +#define M_PI 3.1415926535897932384626433832795 +#endif + +struct fft +{ + fft_complex_t *interleave_buffer; + fft_complex_t *phase_lut; + unsigned *bitinverse_buffer; + unsigned size; +}; + +static unsigned bitswap(unsigned x, unsigned size_log2) +{ + unsigned i; + unsigned ret = 0; + for (i = 0; i < size_log2; i++) + ret |= ((x >> i) & 1) << (size_log2 - i - 1); + return ret; +} + +static void build_bitinverse(unsigned *bitinverse, unsigned size_log2) +{ + unsigned i; + unsigned size = 1 << size_log2; + for (i = 0; i < size; i++) + bitinverse[i] = bitswap(i, size_log2); +} + +static fft_complex_t exp_imag(double phase) +{ + fft_complex_t out = { cos(phase), sin(phase) }; + return out; +} + +static void build_phase_lut(fft_complex_t *out, int size) +{ + int i; + out += size; + for (i = -size; i <= size; i++) + out[i] = exp_imag((M_PI * i) / size); +} + +static void interleave_complex(const unsigned *bitinverse, + fft_complex_t *out, const fft_complex_t *in, + unsigned samples, unsigned step) +{ + unsigned i; + for (i = 0; i < samples; i++, in += step) + out[bitinverse[i]] = *in; +} + +static void interleave_float(const unsigned *bitinverse, + fft_complex_t *out, const float *in, + unsigned samples, unsigned step) +{ + unsigned i; + for (i = 0; i < samples; i++, in += step) + { + unsigned inv_i = bitinverse[i]; + out[inv_i].real = *in; + out[inv_i].imag = 0.0f; + } +} + +static void resolve_float(float *out, const fft_complex_t *in, unsigned samples, + float gain, unsigned step) +{ + unsigned i; + for (i = 0; i < samples; i++, in++, out += step) + *out = gain * in->real; +} + +fft_t *fft_new(unsigned block_size_log2) +{ + fft_t *fft = (fft_t*)calloc(1, sizeof(*fft)); + if (!fft) + return NULL; + + unsigned size = 1 << block_size_log2; + + fft->interleave_buffer = (fft_complex_t*)calloc(size, sizeof(*fft->interleave_buffer)); + fft->bitinverse_buffer = (unsigned*)calloc(size, sizeof(*fft->bitinverse_buffer)); + fft->phase_lut = (fft_complex_t*)calloc(2 * size + 1, sizeof(*fft->phase_lut)); + + if (!fft->interleave_buffer || !fft->bitinverse_buffer || !fft->phase_lut) + goto error; + + fft->size = size; + + build_bitinverse(fft->bitinverse_buffer, block_size_log2); + build_phase_lut(fft->phase_lut, size); + return fft; + +error: + fft_free(fft); + return NULL; +} + +void fft_free(fft_t *fft) +{ + if (!fft) + return; + + free(fft->interleave_buffer); + free(fft->bitinverse_buffer); + free(fft->phase_lut); + free(fft); +} + +static void butterfly(fft_complex_t *a, fft_complex_t *b, fft_complex_t mod) +{ + mod = fft_complex_mul(mod, *b); + *b = fft_complex_sub(*a, mod); + *a = fft_complex_add(*a, mod); +} + +static void butterflies(fft_complex_t *butterfly_buf, + const fft_complex_t *phase_lut, + int phase_dir, unsigned step_size, unsigned samples) +{ + unsigned i, j; + for (i = 0; i < samples; i += step_size << 1) + { + int phase_step = (int)samples * phase_dir / (int)step_size; + for (j = i; j < i + step_size; j++) + butterfly(&butterfly_buf[j], &butterfly_buf[j + step_size], phase_lut[phase_step * (int)(j - i)]); + } +} + +void fft_process_forward_complex(fft_t *fft, + fft_complex_t *out, const fft_complex_t *in, unsigned step) +{ + unsigned step_size; + unsigned samples = fft->size; + interleave_complex(fft->bitinverse_buffer, out, in, samples, step); + + for (step_size = 1; step_size < samples; step_size <<= 1) + { + butterflies(out, + fft->phase_lut + samples, + -1, step_size, samples); + } +} + +void fft_process_forward(fft_t *fft, + fft_complex_t *out, const float *in, unsigned step) +{ + unsigned step_size; + unsigned samples = fft->size; + interleave_float(fft->bitinverse_buffer, out, in, samples, step); + + for (step_size = 1; step_size < fft->size; step_size <<= 1) + { + butterflies(out, + fft->phase_lut + samples, + -1, step_size, samples); + } +} + +void fft_process_inverse(fft_t *fft, + float *out, const fft_complex_t *in, unsigned step) +{ + unsigned step_size; + unsigned samples = fft->size; + interleave_complex(fft->bitinverse_buffer, fft->interleave_buffer, in, samples, 1); + + for (step_size = 1; step_size < samples; step_size <<= 1) + { + butterflies(fft->interleave_buffer, + fft->phase_lut + samples, + 1, step_size, samples); + } + + resolve_float(out, fft->interleave_buffer, samples, 1.0f / samples, step); +} + diff --git a/audio/filters/fft/fft.h b/audio/filters/fft/fft.h new file mode 100644 index 0000000000..3e54ddcfa5 --- /dev/null +++ b/audio/filters/fft/fft.h @@ -0,0 +1,88 @@ +/* RetroArch - A frontend for libretro. + * Copyright (C) 2010-2014 - Hans-Kristian Arntzen + * + * RetroArch is free software: you can redistribute it and/or modify it under the terms + * of the GNU General Public License as published by the Free Software Found- + * ation, either version 3 of the License, or (at your option) any later version. + * + * RetroArch is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; + * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with RetroArch. + * If not, see . + */ + +#ifndef RARCH_FFT_H__ +#define RARCH_FFT_H__ + +typedef struct fft fft_t; + +// C99 would be nice. +typedef struct +{ + float real; + float imag; +} fft_complex_t; + +static inline fft_complex_t fft_complex_mul(fft_complex_t a, + fft_complex_t b) +{ + fft_complex_t out = { + a.real * b.real - a.imag * b.imag, + a.imag * b.real + a.real * b.imag, + }; + + return out; + +} + +static inline fft_complex_t fft_complex_add(fft_complex_t a, + fft_complex_t b) +{ + fft_complex_t out = { + a.real + b.real, + a.imag + b.imag, + }; + + return out; + +} + +static inline fft_complex_t fft_complex_sub(fft_complex_t a, + fft_complex_t b) +{ + fft_complex_t out = { + a.real - b.real, + a.imag - b.imag, + }; + + return out; + +} + +static inline fft_complex_t fft_complex_conj(fft_complex_t a) +{ + fft_complex_t out = { + a.real, -a.imag, + }; + + return out; +} + +fft_t *fft_new(unsigned block_size_log2); + +void fft_free(fft_t *fft); + +void fft_process_forward_complex(fft_t *fft, + fft_complex_t *out, const fft_complex_t *in, unsigned step); + +void fft_process_forward(fft_t *fft, + fft_complex_t *out, const float *in, unsigned step); + +void fft_process_inverse(fft_t *fft, + float *out, const fft_complex_t *in, unsigned step); + + +#endif + diff --git a/griffin/griffin.c b/griffin/griffin.c index 930a7d59a4..4a1638f4e5 100644 --- a/griffin/griffin.c +++ b/griffin/griffin.c @@ -481,6 +481,7 @@ FILTERS #include "../gfx/filters/phosphor2x.c" #include "../audio/filters/echo.c" +#include "../audio/filters/eq.c" #include "../audio/filters/iir.c" #include "../audio/filters/panning.c" #include "../audio/filters/phaser.c"