From b99b288980938e82e8042d1b393ad23a7a528154 Mon Sep 17 00:00:00 2001 From: Themaister Date: Tue, 27 May 2014 22:26:16 +0200 Subject: [PATCH] Implement more of EQ. --- audio/fft/fft.c | 33 ++++---- audio/fft/fft.h | 6 +- audio/filters/eq.c | 199 +++++++++++++++++++++++++++++++++++++++++++-- 3 files changed, 214 insertions(+), 24 deletions(-) diff --git a/audio/fft/fft.c b/audio/fft/fft.c index d091c1730b..e4a33cbd99 100644 --- a/audio/fft/fft.c +++ b/audio/fft/fft.c @@ -62,31 +62,32 @@ static void build_phase_lut(rarch_fft_complex_t *out, int size) static void interleave_complex(const unsigned *bitinverse, rarch_fft_complex_t *out, const rarch_fft_complex_t *in, - unsigned samples) + unsigned samples, unsigned step) { unsigned i; - for (i = 0; i < samples; i++) - out[bitinverse[i]] = in[i]; + for (i = 0; i < samples; i++, in += step) + out[bitinverse[i]] = *in; } static void interleave_float(const unsigned *bitinverse, rarch_fft_complex_t *out, const float *in, - unsigned samples) + unsigned samples, unsigned step) { unsigned i; - for (i = 0; i < samples; i++) + for (i = 0; i < samples; i++, in += step) { unsigned inv_i = bitinverse[i]; - out[inv_i].real = in[i]; + out[inv_i].real = *in; out[inv_i].imag = 0.0f; } } -static void resolve_float(float *out, const rarch_fft_complex_t *in, unsigned samples, float gain) +static void resolve_float(float *out, const rarch_fft_complex_t *in, unsigned samples, + float gain, unsigned step) { unsigned i; - for (i = 0; i < samples; i++) - out[i] = gain * in[i].real; + for (i = 0; i < samples; i++, in++, out += step) + *out = gain * in->real; } rarch_fft_t *rarch_fft_new(unsigned block_size_log2) @@ -147,11 +148,11 @@ static void butterflies(rarch_fft_complex_t *butterfly_buf, } void rarch_fft_process_forward_complex(rarch_fft_t *fft, - rarch_fft_complex_t *out, const rarch_fft_complex_t *in) + rarch_fft_complex_t *out, const rarch_fft_complex_t *in, unsigned step) { unsigned step_size; unsigned samples = fft->size; - interleave_complex(fft->bitinverse_buffer, out, in, fft->size); + interleave_complex(fft->bitinverse_buffer, out, in, fft->size, step); for (step_size = 1; step_size < samples; step_size <<= 1) { @@ -162,11 +163,11 @@ void rarch_fft_process_forward_complex(rarch_fft_t *fft, } void rarch_fft_process_forward(rarch_fft_t *fft, - rarch_fft_complex_t *out, const float *in) + rarch_fft_complex_t *out, const float *in, unsigned step) { unsigned step_size; unsigned samples = fft->size; - interleave_float(fft->bitinverse_buffer, out, in, fft->size); + interleave_float(fft->bitinverse_buffer, out, in, fft->size, step); for (step_size = 1; step_size < fft->size; step_size <<= 1) { @@ -177,11 +178,11 @@ void rarch_fft_process_forward(rarch_fft_t *fft, } void rarch_fft_process_inverse(rarch_fft_t *fft, - float *out, const rarch_fft_complex_t *in) + float *out, const rarch_fft_complex_t *in, unsigned step) { unsigned step_size; unsigned samples = fft->size; - interleave_complex(fft->bitinverse_buffer, fft->interleave_buffer, in, fft->size); + interleave_complex(fft->bitinverse_buffer, fft->interleave_buffer, in, fft->size, step); for (step_size = 1; step_size < samples; step_size <<= 1) { @@ -190,6 +191,6 @@ void rarch_fft_process_inverse(rarch_fft_t *fft, 1, step_size, samples); } - resolve_float(out, fft->interleave_buffer, samples, 1.0f / samples); + resolve_float(out, fft->interleave_buffer, samples, 1.0f / samples, step); } diff --git a/audio/fft/fft.h b/audio/fft/fft.h index 6b68526e6b..96d18cd9b5 100644 --- a/audio/fft/fft.h +++ b/audio/fft/fft.h @@ -75,13 +75,13 @@ rarch_fft_t *rarch_fft_new(unsigned block_size_log2); void rarch_fft_free(rarch_fft_t *fft); void rarch_fft_process_forward_complex(rarch_fft_t *fft, - rarch_fft_complex_t *out, const rarch_fft_complex_t *in); + rarch_fft_complex_t *out, const rarch_fft_complex_t *in, unsigned step); void rarch_fft_process_forward(rarch_fft_t *fft, - rarch_fft_complex_t *out, const float *in); + rarch_fft_complex_t *out, const float *in, unsigned step); void rarch_fft_process_inverse(rarch_fft_t *fft, - float *out, const rarch_fft_complex_t *in); + float *out, const rarch_fft_complex_t *in, unsigned step); #endif diff --git a/audio/filters/eq.c b/audio/filters/eq.c index 7cc736cd7a..772de6fc95 100644 --- a/audio/filters/eq.c +++ b/audio/filters/eq.c @@ -16,6 +16,7 @@ #include "dspfilter.h" #include #include +#include #include "../fft/fft.c" @@ -31,10 +32,17 @@ struct eq_data float *save; float *block; rarch_fft_complex_t *filter; + rarch_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; @@ -44,6 +52,7 @@ static void eq_free(void *data) rarch_fft_free(eq->fft); free(eq->save); free(eq->block); + free(eq->fftblock); free(eq->filter); free(eq); } @@ -52,6 +61,178 @@ 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++) + { + rarch_fft_process_forward(eq->fft, eq->fftblock, eq->block + c, 2); + for (i = 0; i < 2 * eq->block_size; i++) + eq->fftblock[i] = rarch_fft_complex_mul(eq->fftblock[i], eq->filter[i]); + rarch_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(rarch_fft_complex_t *response, + const struct eq_gain *gains, unsigned num_gains, unsigned samples) +{ + unsigned i; + + // DC and Nyquist get 0 gain. (This will get smeared out good with windowing later though ...) + response[0].real = 0.0f; + response[0].imag = 0.0f; + response[samples].real = 0.0f; + response[samples].imag = 0.0f; + + 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 = 1; 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 + { + end_freq = 1.0f; + end_gain = 1.0f; + } + } + + float 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 = gain; + } +} + +static void create_filter(struct eq_data *eq, unsigned size_log2, + struct eq_gain *gains, unsigned num_gains) +{ + int i; + int half_block_size = eq->block_size >> 1; + + rarch_fft_t *fft = rarch_fft_new(size_log2); + float *time_filter = (float*)calloc(eq->block_size * 2, sizeof(*time_filter)); + if (!fft || !time_filter) + goto end; + + 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. + rarch_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] *= cos(phase * M_PI); + } + + // Debugging. +#if 1 + FILE *file = fopen("/tmp/test.txt", "w"); + if (file) + { + for (i = 0; i < (int)eq->block_size; i++) + fprintf(file, "%.6f", time_filter[i]); + fclose(file); + } +#endif + + // Padded FFT to create our FFT filter. + rarch_fft_process_forward(eq->fft, eq->filter, time_filter, 1); + +end: + rarch_fft_free(fft); + free(time_filter); } static void *eq_init(const struct dspfilter_info *info, @@ -66,14 +247,22 @@ static void *eq_init(const struct dspfilter_info *info, eq->block_size = size; - eq->save = (float*)calloc(2 * size, sizeof(*eq->save)); - eq->block = (float*)calloc(2 * size, 2 * sizeof(*eq->block)); - eq->filter = (rarch_fft_complex_t*)calloc(2 * size, sizeof(*eq->filter)); - eq->fft = rarch_fft_new(size_log2 + 1); + eq->save = (float*)calloc( size, 2 * sizeof(*eq->save)); + eq->block = (float*)calloc(2 * size, 2 * sizeof(*eq->block)); + eq->fftblock = (rarch_fft_complex_t*)calloc(2 * size, sizeof(*eq->fftblock)); + eq->filter = (rarch_fft_complex_t*)calloc(2 * size, sizeof(*eq->filter)); - if (!eq->fft || !eq->save || !eq->block || !eq->filter) + // Use an FFT which is twice the block size with zero-padding + // to make circular convolution => proper convolution. + eq->fft = rarch_fft_new(size_log2 + 1); + + if (!eq->fft || !eq->fftblock || !eq->save || !eq->block || !eq->filter) goto error; + struct eq_gain *gains = NULL; + unsigned num_gains = 0; + create_filter(eq, size_log2, gains, num_gains); + return eq; error: