RetroArch/audio/sinc.c

261 lines
7.0 KiB
C
Raw Normal View History

2012-02-23 22:53:18 +00:00
/* 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/>.
*/
// Bog-standard windowed SINC implementation.
// Only suitable as an upsampler, as there is no low-pass filter stage.
#include "resampler.h"
#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <malloc.h>
#include <string.h>
2012-02-25 00:47:23 +00:00
#ifndef RESAMPLER_TEST
#include "../general.h"
#else
#define SSNES_LOG(...)
#endif
2012-02-23 22:53:18 +00:00
#if __SSE__
#include <xmmintrin.h>
#endif
#define PHASE_BITS 8
#define SUBPHASE_BITS 16
#define PHASES (1 << PHASE_BITS)
#define PHASES_SHIFT (SUBPHASE_BITS)
#define SUBPHASES (1 << SUBPHASE_BITS)
#define SUBPHASES_SHIFT 0
#define SUBPHASES_MASK ((1 << SUBPHASE_BITS) - 1)
#define PHASES_WRAP (1 << (PHASE_BITS + SUBPHASE_BITS))
#define FRAMES_SHIFT (PHASE_BITS + SUBPHASE_BITS)
2012-02-25 00:47:23 +00:00
#define SIDELOBES 16
2012-02-24 20:01:29 +00:00
#define TAPS (SIDELOBES * 2)
#define PHASE_INDEX 0
#define DELTA_INDEX 1
2012-02-23 22:53:18 +00:00
struct ssnes_resampler
{
2012-02-24 20:01:29 +00:00
float phase_table[PHASES][2][2 * SIDELOBES];
2012-02-23 22:53:18 +00:00
float buffer_l[2 * SIDELOBES];
float buffer_r[2 * SIDELOBES];
uint32_t time;
};
2012-02-25 13:31:36 +00:00
void resampler_preinit(ssnes_resampler_t *re, double omega, double *samples_offset)
2012-02-25 13:02:56 +00:00
{
*samples_offset = SIDELOBES + 1;
for (int i = 0; i < 2 * SIDELOBES; i++)
{
re->buffer_l[i] = cos((i - (SIDELOBES - 1)) * omega);
re->buffer_r[i] = re->buffer_l[i];
}
re->time = 0;
}
2012-02-23 22:53:18 +00:00
static inline double sinc(double val)
{
if (fabs(val) < 0.00001)
return 1.0;
else
return sin(val) / val;
}
2012-02-25 15:45:06 +00:00
static inline double lanzcos(double index)
2012-02-23 22:53:18 +00:00
{
2012-02-25 15:45:06 +00:00
return sinc(index);
2012-02-23 22:53:18 +00:00
}
static void init_sinc_table(ssnes_resampler_t *resamp)
{
2012-02-24 20:01:29 +00:00
// Sinc phases: [..., p + 3, p + 2, p + 1, p + 0, p - 1, p - 2, p - 3, p - 4, ...]
for (int i = 0; i < PHASES; i++)
2012-02-23 22:53:18 +00:00
{
2012-02-25 15:33:33 +00:00
for (int j = 0; j < TAPS; j++)
2012-02-23 22:53:18 +00:00
{
2012-02-24 20:01:29 +00:00
double p = (double)i / PHASES;
double sinc_phase = M_PI * (p + (SIDELOBES - 1 - j));
2012-02-25 15:45:06 +00:00
resamp->phase_table[i][PHASE_INDEX][j] = sinc(sinc_phase) *
lanzcos(sinc_phase / SIDELOBES);
2012-02-23 22:53:18 +00:00
}
}
// Optimize linear interpolation.
2012-02-24 20:01:29 +00:00
for (int i = 0; i < PHASES - 1; i++)
{
2012-02-25 15:33:33 +00:00
for (int j = 0; j < TAPS; j++)
2012-02-24 20:01:29 +00:00
{
resamp->phase_table[i][DELTA_INDEX][j] =
(resamp->phase_table[i + 1][PHASE_INDEX][j] - resamp->phase_table[i][PHASE_INDEX][j]) / SUBPHASES;
}
}
2012-02-25 15:33:33 +00:00
// Interpolation between [PHASES - 1] => [PHASES]
for (int j = 0; j < TAPS; j++)
{
double p = 1.0;
double sinc_phase = M_PI * (p + (SIDELOBES - 1 - j));
2012-02-25 15:45:06 +00:00
double phase = sinc(sinc_phase) * lanzcos(sinc_phase / SIDELOBES);
2012-02-25 15:33:33 +00:00
float result = (phase - resamp->phase_table[PHASES - 1][PHASE_INDEX][j]) / SUBPHASES;
resamp->phase_table[PHASES - 1][DELTA_INDEX][j] = result;
}
2012-02-23 22:53:18 +00:00
}
ssnes_resampler_t *resampler_new(void)
{
2012-02-23 22:57:35 +00:00
ssnes_resampler_t *re = (ssnes_resampler_t*)memalign(16, sizeof(*re));
2012-02-23 22:53:18 +00:00
if (!re)
return NULL;
memset(re, 0, sizeof(*re));
init_sinc_table(re);
2012-02-24 20:01:29 +00:00
2012-02-25 20:47:40 +00:00
#if __SSE__
2012-02-24 20:01:29 +00:00
SSNES_LOG("Sinc resampler [SSE]\n");
#else
SSNES_LOG("Sinc resampler [C]\n");
#endif
2012-02-23 22:53:18 +00:00
return re;
}
#if __SSE__
2012-02-23 22:57:35 +00:00
static void process_sinc(ssnes_resampler_t *resamp, float *out_buffer)
2012-02-23 22:53:18 +00:00
{
__m128 sum_l = _mm_setzero_ps();
__m128 sum_r = _mm_setzero_ps();
const float *buffer_l = resamp->buffer_l;
const float *buffer_r = resamp->buffer_r;
unsigned phase = resamp->time >> PHASES_SHIFT;
unsigned delta = (resamp->time >> SUBPHASES_SHIFT) & SUBPHASES_MASK;
2012-02-24 20:01:29 +00:00
__m128 delta_f = _mm_set1_ps(delta);
2012-02-23 22:53:18 +00:00
2012-02-24 20:01:29 +00:00
const float *phase_table = resamp->phase_table[phase][PHASE_INDEX];
const float *delta_table = resamp->phase_table[phase][DELTA_INDEX];
2012-02-23 22:53:18 +00:00
2012-02-24 20:01:29 +00:00
for (unsigned i = 0; i < TAPS; i += 4)
2012-02-23 22:53:18 +00:00
{
2012-02-24 20:01:29 +00:00
__m128 buf_l = _mm_load_ps(buffer_l + i);
__m128 buf_r = _mm_load_ps(buffer_r + i);
2012-02-23 22:53:18 +00:00
2012-02-24 20:01:29 +00:00
__m128 phases = _mm_load_ps(phase_table + i);
__m128 deltas = _mm_load_ps(delta_table + i);
2012-02-23 22:53:18 +00:00
2012-02-24 20:01:29 +00:00
__m128 sinc = _mm_add_ps(phases, _mm_mul_ps(deltas, delta_f));
2012-02-23 22:53:18 +00:00
2012-02-24 20:01:29 +00:00
sum_l = _mm_add_ps(sum_l, _mm_mul_ps(buf_l, sinc));
sum_r = _mm_add_ps(sum_r, _mm_mul_ps(buf_r, sinc));
2012-02-23 22:53:18 +00:00
}
2012-02-25 20:47:40 +00:00
// Them annoying shuffles :V
// sum_l = { l3, l2, l1, l0 }
// sum_r = { r3, r2, r1, r0 }
__m128 sum = _mm_add_ps(_mm_shuffle_ps(sum_l, sum_r, _MM_SHUFFLE(1, 0, 1, 0)),
_mm_shuffle_ps(sum_l, sum_r, _MM_SHUFFLE(3, 2, 3, 2)));
// sum = { r1, r0, l1, l0 } + { r3, r2, l3, l2 }
// sum = { R1, R0, L1, L0 }
sum = _mm_add_ps(_mm_shuffle_ps(sum, sum, _MM_SHUFFLE(3, 3, 1, 1)), sum);
// sum = {R1, R1, L1, L1 } + { R1, R0, L1, L0 }
// sum = { X, R, X, L }
// Store L
_mm_store_ss(out_buffer + 0, sum);
// movehl { X, R, X, L } == { X, R, X, R }
_mm_store_ss(out_buffer + 1, _mm_movehl_ps(sum, sum));
2012-02-23 22:53:18 +00:00
}
#else // Plain ol' C99
2012-02-23 22:57:35 +00:00
static void process_sinc(ssnes_resampler_t *resamp, float *out_buffer)
2012-02-23 22:53:18 +00:00
{
float sum_l = 0.0f;
float sum_r = 0.0f;
const float *buffer_l = resamp->buffer_l;
const float *buffer_r = resamp->buffer_r;
unsigned phase = resamp->time >> PHASES_SHIFT;
unsigned delta = (resamp->time >> SUBPHASES_SHIFT) & SUBPHASES_MASK;
2012-02-24 20:01:29 +00:00
float delta_f = (float)delta;
2012-02-23 22:53:18 +00:00
2012-02-24 20:01:29 +00:00
const float *phase_table = resamp->phase_table[phase][PHASE_INDEX];
const float *delta_table = resamp->phase_table[phase][DELTA_INDEX];
2012-02-23 22:53:18 +00:00
2012-02-24 20:01:29 +00:00
for (unsigned i = 0; i < TAPS; i++)
2012-02-23 22:53:18 +00:00
{
2012-02-24 20:01:29 +00:00
float sinc_val = phase_table[i] + delta_f * delta_table[i];
sum_l += buffer_l[i] * sinc_val;
sum_r += buffer_r[i] * sinc_val;
2012-02-23 22:53:18 +00:00
}
out_buffer[0] = sum_l;
out_buffer[1] = sum_r;
}
#endif
void resampler_process(ssnes_resampler_t *re, struct resampler_data *data)
{
uint32_t ratio = PHASES_WRAP / data->ratio;
const float *input = data->data_in;
float *output = data->data_out;
size_t frames = data->input_frames;
size_t out_frames = 0;
while (frames)
{
process_sinc(re, output);
output += 2;
out_frames++;
re->time += ratio;
while (re->time >= PHASES_WRAP)
{
memmove(re->buffer_l, re->buffer_l + 1,
sizeof(re->buffer_l) - sizeof(float));
memmove(re->buffer_r, re->buffer_r + 1,
sizeof(re->buffer_r) - sizeof(float));
re->buffer_l[2 * SIDELOBES - 1] = *input++;
re->buffer_r[2 * SIDELOBES - 1] = *input++;
re->time -= PHASES_WRAP;
frames--;
}
}
data->output_frames = out_frames;
}
void resampler_free(ssnes_resampler_t *re)
{
free(re);
}