Implement more of EQ.

This commit is contained in:
Themaister 2014-05-27 22:26:16 +02:00
parent ce14f2f517
commit b99b288980
3 changed files with 214 additions and 24 deletions

View File

@ -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);
}

View File

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

View File

@ -16,6 +16,7 @@
#include "dspfilter.h"
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#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: