274 lines
7.1 KiB
C
274 lines
7.1 KiB
C
/* SSNES - A Super Nintendo Entertainment System (SNES) Emulator frontend for libsnes.
|
|
* Copyright (C) 2010-2012 - Hans-Kristian Arntzen
|
|
*
|
|
* Some code herein may be based on code found in BSNES.
|
|
*
|
|
* SSNES 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.
|
|
*
|
|
* SSNES 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 SSNES.
|
|
* If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "../resampler.h"
|
|
#include "../utils.h"
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
#include <complex.h>
|
|
#include <assert.h>
|
|
#include <stdbool.h>
|
|
|
|
static void gen_signal(float *out, double omega, double bias_samples, size_t samples)
|
|
{
|
|
for (size_t i = 0; i < samples; i += 2)
|
|
{
|
|
out[i + 0] = cos(((i >> 1) + bias_samples) * omega);
|
|
out[i + 1] = out[i + 0];
|
|
}
|
|
}
|
|
|
|
struct snr_result
|
|
{
|
|
double snr;
|
|
double gain;
|
|
double phase;
|
|
};
|
|
|
|
static unsigned bitrange(unsigned len)
|
|
{
|
|
unsigned ret = 0;
|
|
while ((len >>= 1))
|
|
ret++;
|
|
|
|
return ret;
|
|
}
|
|
|
|
static unsigned bitswap(unsigned i, unsigned range)
|
|
{
|
|
unsigned ret = 0;
|
|
for (unsigned shifts = 0; shifts < range; shifts++)
|
|
ret |= i & (1 << (range - shifts - 1)) ? (1 << shifts) : 0;
|
|
|
|
return ret;
|
|
}
|
|
|
|
// When interleaving the butterfly buffer, addressing puts bits in reverse.
|
|
// [0, 1, 2, 3, 4, 5, 6, 7] => [0, 4, 2, 6, 1, 5, 3, 7]
|
|
static void interleave(complex double *butterfly_buf, size_t samples)
|
|
{
|
|
unsigned range = bitrange(samples);
|
|
for (unsigned i = 0; i < samples; i++)
|
|
{
|
|
unsigned target = bitswap(i, range);
|
|
if (target > i)
|
|
{
|
|
complex double tmp = butterfly_buf[target];
|
|
butterfly_buf[target] = butterfly_buf[i];
|
|
butterfly_buf[i] = tmp;
|
|
}
|
|
}
|
|
}
|
|
|
|
static complex double gen_phase(double index)
|
|
{
|
|
return cexp(-M_PI * I * index);
|
|
}
|
|
|
|
static void butterfly(complex double *a, complex double *b, complex double mod)
|
|
{
|
|
mod *= *b;
|
|
complex double a_ = *a + mod;
|
|
complex double b_ = *a - mod;
|
|
*a = a_;
|
|
*b = b_;
|
|
}
|
|
|
|
static void butterflies(complex double *butterfly_buf, size_t step_size, size_t samples)
|
|
{
|
|
for (unsigned i = 0; i < samples; i += 2 * step_size)
|
|
for (unsigned j = i; j < i + step_size; j++)
|
|
butterfly(&butterfly_buf[j], &butterfly_buf[j + step_size], gen_phase((double)(j - i) / step_size));
|
|
}
|
|
|
|
static void calculate_fft(const float *data, complex double *butterfly_buf, size_t samples)
|
|
{
|
|
// Enforce POT.
|
|
assert((samples & (samples - 1)) == 0);
|
|
|
|
for (unsigned i = 0; i < samples; i++)
|
|
butterfly_buf[i] = data[2 * i];
|
|
|
|
// Interleave buffer to work with FFT.
|
|
interleave(butterfly_buf, samples);
|
|
|
|
// Fly, lovely butterflies! :D
|
|
for (unsigned step_size = 1; step_size < samples; step_size *= 2)
|
|
butterflies(butterfly_buf, step_size, samples);
|
|
|
|
// We only have real data.
|
|
for (unsigned i = 1; i < samples / 2; i++)
|
|
butterfly_buf[i] += butterfly_buf[samples - i];
|
|
|
|
// Normalize amplitudes.
|
|
for (unsigned i = 0; i < samples / 2; i++)
|
|
butterfly_buf[i] /= (double)samples;
|
|
}
|
|
|
|
static void test_fft(void)
|
|
{
|
|
fprintf(stderr, "Sanity checking FFT ...\n");
|
|
float signal[32];
|
|
complex double butterfly_buf[16];
|
|
|
|
const float freqs[] = {
|
|
1.0, 4.0, 6.0,
|
|
};
|
|
|
|
for (unsigned i = 0; i < 16; i++)
|
|
{
|
|
signal[2 * i] = 0.0;
|
|
for (unsigned j = 0; j < sizeof(freqs) / sizeof(freqs[0]); j++)
|
|
signal[2 * i] += cos(2.0 * M_PI * i * freqs[j] / 16.0);
|
|
}
|
|
|
|
calculate_fft(signal, butterfly_buf, 16);
|
|
|
|
printf("FFT: { ");
|
|
for (unsigned i = 0; i < 7; i++)
|
|
printf("%4.2lf, ", cabs(butterfly_buf[i]));
|
|
printf("%4.2lf }\n", cabs(butterfly_buf[7]));
|
|
}
|
|
|
|
// This doesn't yet take account for slight phase distortions,
|
|
// so reported SNR is lower than reality.
|
|
static void calculate_snr(struct snr_result *res,
|
|
unsigned in_rate,
|
|
const float *resamp, complex double *butterfly_buf, size_t samples)
|
|
{
|
|
samples >>= 1;
|
|
calculate_fft(resamp, butterfly_buf, samples);
|
|
|
|
complex double phase = butterfly_buf[in_rate];
|
|
res->phase = carg(phase);
|
|
|
|
double signal = cabs(phase * phase);
|
|
butterfly_buf[in_rate] = 0.0;
|
|
|
|
double noise = 0.0;
|
|
for (unsigned i = 0; i < samples / 2; i++)
|
|
noise += cabs(butterfly_buf[i] * butterfly_buf[i]);
|
|
|
|
res->snr = 10.0 * log10(signal / noise);
|
|
res->gain = 10.0 * log10(signal);
|
|
}
|
|
|
|
int main(int argc, char *argv[])
|
|
{
|
|
if (argc != 2)
|
|
{
|
|
fprintf(stderr, "Usage: %s <ratio> (out-rate is fixed for FFT).\n", argv[0]);
|
|
return 1;
|
|
}
|
|
|
|
double ratio = strtod(argv[1], NULL);
|
|
|
|
const unsigned fft_samples = 1024 * 128;
|
|
unsigned out_rate = fft_samples;
|
|
unsigned in_rate = out_rate / ratio;
|
|
ratio = (double)out_rate / in_rate;
|
|
|
|
if (ratio <= 1.0)
|
|
{
|
|
fprintf(stderr, "Ratio too low ...\n");
|
|
return 1;
|
|
}
|
|
|
|
static const unsigned freq_list[] = {
|
|
30, 50,
|
|
100, 150,
|
|
200, 250,
|
|
300, 350,
|
|
400, 450,
|
|
500, 550,
|
|
600, 650,
|
|
700, 800,
|
|
900, 1000,
|
|
1100, 1200,
|
|
1300, 1500,
|
|
1600, 1700,
|
|
1800, 1900,
|
|
2000, 2100,
|
|
2200, 2300,
|
|
2500, 3000,
|
|
3500, 4000,
|
|
4500, 5000,
|
|
5500, 6000,
|
|
6500, 7000,
|
|
7500, 8000,
|
|
9000, 9500,
|
|
10000, 11000,
|
|
12000, 13000,
|
|
14000, 15000,
|
|
16000, 17000,
|
|
18000, 19000,
|
|
20000, 21000,
|
|
22000,
|
|
};
|
|
|
|
unsigned samples = in_rate * 2;
|
|
float *input = calloc(sizeof(float), samples);
|
|
float *output = calloc(sizeof(float), (fft_samples + 1) * 2);
|
|
complex double *butterfly_buf = calloc(sizeof(complex double), fft_samples);
|
|
bool warned = false;
|
|
assert(input);
|
|
assert(output);
|
|
|
|
ssnes_resampler_t *re = resampler_new();
|
|
assert(re);
|
|
|
|
test_fft();
|
|
|
|
for (unsigned i = 0; i < sizeof(freq_list) / sizeof(freq_list[0]) && freq_list[i] < 0.5f * in_rate; i++)
|
|
{
|
|
double omega = 2.0 * M_PI * freq_list[i] / in_rate;
|
|
double sample_offset;
|
|
resampler_preinit(re, omega, &sample_offset);
|
|
gen_signal(input, omega, sample_offset, samples);
|
|
|
|
struct resampler_data data = {
|
|
.data_in = input,
|
|
.data_out = output,
|
|
.input_frames = in_rate,
|
|
.ratio = ratio,
|
|
};
|
|
|
|
resampler_process(re, &data);
|
|
|
|
unsigned out_samples = data.output_frames * 2;
|
|
|
|
if (out_samples != fft_samples * 2 && !warned)
|
|
{
|
|
fprintf(stderr, "Out samples != fft_samples ... %u / %u\n", out_samples, fft_samples * 2);
|
|
warned = true;
|
|
}
|
|
|
|
struct snr_result res;
|
|
calculate_snr(&res, freq_list[i], output, butterfly_buf, fft_samples * 2);
|
|
|
|
printf("SNR @ %5u Hz: %6.2lf dB, Gain: %6.1lf dB, Phase: %6.4f rad\n",
|
|
freq_list[i], res.snr, res.gain, res.phase);
|
|
}
|
|
|
|
resampler_free(re);
|
|
free(input);
|
|
free(output);
|
|
free(butterfly_buf);
|
|
}
|
|
|