From f44ba092651aa75055e109e04b4164ea92ae7fdc Mon Sep 17 00:00:00 2001 From: Lennart Poettering Date: Mon, 19 Jun 2006 21:53:48 +0000 Subject: big s/polyp/pulse/g git-svn-id: file:///home/lennart/svn/public/pulseaudio/trunk@1033 fefdeb5f-60dc-0310-8127-8f9354f1896f --- src/pulsecore/resampler.c | 618 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 618 insertions(+) create mode 100644 src/pulsecore/resampler.c (limited to 'src/pulsecore/resampler.c') diff --git a/src/pulsecore/resampler.c b/src/pulsecore/resampler.c new file mode 100644 index 00000000..23cdf381 --- /dev/null +++ b/src/pulsecore/resampler.c @@ -0,0 +1,618 @@ +/* $Id$ */ + +/*** + This file is part of PulseAudio. + + PulseAudio is free software; you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License as published + by the Free Software Foundation; either version 2 of the License, + or (at your option) any later version. + + PulseAudio 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 Lesser General Public License + along with PulseAudio; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 + USA. +***/ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include +#include + +#include +#include +#include + +#include + +#include +#include + +#include "resampler.h" + +struct pa_resampler { + pa_resample_method_t resample_method; + pa_sample_spec i_ss, o_ss; + pa_channel_map i_cm, o_cm; + size_t i_fz, o_fz; + pa_memblock_stat *memblock_stat; + + void (*impl_free)(pa_resampler *r); + void (*impl_update_input_rate)(pa_resampler *r, uint32_t rate); + void (*impl_run)(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out); + void *impl_data; +}; + +struct impl_libsamplerate { + float* buf1, *buf2, *buf3, *buf4; + unsigned buf1_samples, buf2_samples, buf3_samples, buf4_samples; + + pa_convert_to_float32ne_func_t to_float32ne_func; + pa_convert_from_float32ne_func_t from_float32ne_func; + SRC_STATE *src_state; + + int map_table[PA_CHANNELS_MAX][PA_CHANNELS_MAX]; + int map_required; +}; + +struct impl_trivial { + unsigned o_counter; + unsigned i_counter; +}; + +static int libsamplerate_init(pa_resampler*r); +static int trivial_init(pa_resampler*r); + +pa_resampler* pa_resampler_new( + const pa_sample_spec *a, + const pa_channel_map *am, + const pa_sample_spec *b, + const pa_channel_map *bm, + pa_memblock_stat *s, + pa_resample_method_t resample_method) { + + pa_resampler *r = NULL; + + assert(a); + assert(b); + assert(pa_sample_spec_valid(a)); + assert(pa_sample_spec_valid(b)); + assert(resample_method != PA_RESAMPLER_INVALID); + + r = pa_xnew(pa_resampler, 1); + r->impl_data = NULL; + r->memblock_stat = s; + r->resample_method = resample_method; + + r->impl_free = NULL; + r->impl_update_input_rate = NULL; + r->impl_run = NULL; + + /* Fill sample specs */ + r->i_ss = *a; + r->o_ss = *b; + + if (am) + r->i_cm = *am; + else + pa_channel_map_init_auto(&r->i_cm, r->i_ss.channels, PA_CHANNEL_MAP_DEFAULT); + + if (bm) + r->o_cm = *bm; + else + pa_channel_map_init_auto(&r->o_cm, r->o_ss.channels, PA_CHANNEL_MAP_DEFAULT); + + r->i_fz = pa_frame_size(a); + r->o_fz = pa_frame_size(b); + + /* Choose implementation */ + if (a->channels != b->channels || + a->format != b->format || + !pa_channel_map_equal(&r->i_cm, &r->o_cm) || + resample_method != PA_RESAMPLER_TRIVIAL) { + + /* Use the libsamplerate based resampler for the complicated cases */ + if (resample_method == PA_RESAMPLER_TRIVIAL) + r->resample_method = PA_RESAMPLER_SRC_ZERO_ORDER_HOLD; + + if (libsamplerate_init(r) < 0) + goto fail; + + } else { + /* Use our own simple non-fp resampler for the trivial cases and when the user selects it */ + if (trivial_init(r) < 0) + goto fail; + } + + return r; + +fail: + if (r) + pa_xfree(r); + + return NULL; +} + +void pa_resampler_free(pa_resampler *r) { + assert(r); + + if (r->impl_free) + r->impl_free(r); + + pa_xfree(r); +} + +void pa_resampler_set_input_rate(pa_resampler *r, uint32_t rate) { + assert(r); + assert(rate > 0); + + if (r->i_ss.rate == rate) + return; + + r->i_ss.rate = rate; + + if (r->impl_update_input_rate) + r->impl_update_input_rate(r, rate); +} + +void pa_resampler_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) { + assert(r && in && out && r->impl_run); + + r->impl_run(r, in, out); +} + +size_t pa_resampler_request(pa_resampler *r, size_t out_length) { + assert(r); + + return (((out_length / r->o_fz)*r->i_ss.rate)/r->o_ss.rate) * r->i_fz; +} + +pa_resample_method_t pa_resampler_get_method(pa_resampler *r) { + assert(r); + return r->resample_method; +} + +static const char * const resample_methods[] = { + "src-sinc-best-quality", + "src-sinc-medium-quality", + "src-sinc-fastest", + "src-zero-order-hold", + "src-linear", + "trivial" +}; + +const char *pa_resample_method_to_string(pa_resample_method_t m) { + + if (m < 0 || m >= PA_RESAMPLER_MAX) + return NULL; + + return resample_methods[m]; +} + +pa_resample_method_t pa_parse_resample_method(const char *string) { + pa_resample_method_t m; + + assert(string); + + for (m = 0; m < PA_RESAMPLER_MAX; m++) + if (!strcmp(string, resample_methods[m])) + return m; + + return PA_RESAMPLER_INVALID; +} + + +/*** libsamplerate based implementation ***/ + +static void libsamplerate_free(pa_resampler *r) { + struct impl_libsamplerate *u; + + assert(r); + assert(r->impl_data); + + u = r->impl_data; + + if (u->src_state) + src_delete(u->src_state); + + pa_xfree(u->buf1); + pa_xfree(u->buf2); + pa_xfree(u->buf3); + pa_xfree(u->buf4); + pa_xfree(u); +} + +static void calc_map_table(pa_resampler *r) { + struct impl_libsamplerate *u; + unsigned oc; + assert(r); + assert(r->impl_data); + + u = r->impl_data; + + if (!(u->map_required = (!pa_channel_map_equal(&r->i_cm, &r->o_cm) || r->i_ss.channels != r->o_ss.channels))) + return; + + for (oc = 0; oc < r->o_ss.channels; oc++) { + unsigned ic, i = 0; + + for (ic = 0; ic < r->i_ss.channels; ic++) { + pa_channel_position_t a, b; + + a = r->i_cm.map[ic]; + b = r->o_cm.map[oc]; + + if (a == b || + (a == PA_CHANNEL_POSITION_MONO && b == PA_CHANNEL_POSITION_LEFT) || + (a == PA_CHANNEL_POSITION_MONO && b == PA_CHANNEL_POSITION_RIGHT) || + (a == PA_CHANNEL_POSITION_LEFT && b == PA_CHANNEL_POSITION_MONO) || + (a == PA_CHANNEL_POSITION_RIGHT && b == PA_CHANNEL_POSITION_MONO)) + + u->map_table[oc][i++] = ic; + } + + /* Add an end marker */ + if (i < PA_CHANNELS_MAX) + u->map_table[oc][i] = -1; + } +} + +static float * convert_to_float(pa_resampler *r, void *input, unsigned n_frames) { + struct impl_libsamplerate *u; + unsigned n_samples; + + assert(r); + assert(input); + assert(r->impl_data); + u = r->impl_data; + + /* Convert the incoming sample into floats and place them in buf1 */ + + if (!u->to_float32ne_func) + return input; + + n_samples = n_frames * r->i_ss.channels; + + if (u->buf1_samples < n_samples) + u->buf1 = pa_xrealloc(u->buf1, sizeof(float) * (u->buf1_samples = n_samples)); + + u->to_float32ne_func(n_samples, input, u->buf1); + + return u->buf1; +} + +static float *remap_channels(pa_resampler *r, float *input, unsigned n_frames) { + struct impl_libsamplerate *u; + unsigned n_samples; + int i_skip, o_skip; + unsigned oc; + + assert(r); + assert(input); + assert(r->impl_data); + u = r->impl_data; + + /* Remap channels and place the result int buf2 */ + + if (!u->map_required) + return input; + + n_samples = n_frames * r->o_ss.channels; + + if (u->buf2_samples < n_samples) + u->buf2 = pa_xrealloc(u->buf2, sizeof(float) * (u->buf2_samples = n_samples)); + + memset(u->buf2, 0, n_samples * sizeof(float)); + + o_skip = sizeof(float) * r->o_ss.channels; + i_skip = sizeof(float) * r->i_ss.channels; + + for (oc = 0; oc < r->o_ss.channels; oc++) { + unsigned i; + static const float one = 1.0; + + for (i = 0; i < PA_CHANNELS_MAX && u->map_table[oc][i] >= 0; i++) + oil_vectoradd_f32( + u->buf2 + oc, o_skip, + u->buf2 + oc, o_skip, + input + u->map_table[oc][i], i_skip, + n_frames, + &one, &one); + } + + return u->buf2; +} + +static float *resample(pa_resampler *r, float *input, unsigned *n_frames) { + struct impl_libsamplerate *u; + SRC_DATA data; + unsigned out_n_frames, out_n_samples; + int ret; + + assert(r); + assert(input); + assert(n_frames); + assert(r->impl_data); + u = r->impl_data; + + /* Resample the data and place the result in buf3 */ + + if (!u->src_state) + return input; + + out_n_frames = (*n_frames*r->o_ss.rate/r->i_ss.rate)+1024; + out_n_samples = out_n_frames * r->o_ss.channels; + + if (u->buf3_samples < out_n_samples) + u->buf3 = pa_xrealloc(u->buf3, sizeof(float) * (u->buf3_samples = out_n_samples)); + + data.data_in = input; + data.input_frames = *n_frames; + + data.data_out = u->buf3; + data.output_frames = out_n_frames; + + data.src_ratio = (double) r->o_ss.rate / r->i_ss.rate; + data.end_of_input = 0; + + ret = src_process(u->src_state, &data); + assert(ret == 0); + assert((unsigned) data.input_frames_used == *n_frames); + + *n_frames = data.output_frames_gen; + + return u->buf3; +} + +static void *convert_from_float(pa_resampler *r, float *input, unsigned n_frames) { + struct impl_libsamplerate *u; + unsigned n_samples; + + assert(r); + assert(input); + assert(r->impl_data); + u = r->impl_data; + + /* Convert the data into the correct sample type and place the result in buf4 */ + + if (!u->from_float32ne_func) + return input; + + n_samples = n_frames * r->o_ss.channels; + + if (u->buf4_samples < n_samples) + u->buf4 = pa_xrealloc(u->buf4, sizeof(float) * (u->buf4_samples = n_samples)); + + u->from_float32ne_func(n_samples, input, u->buf4); + + return u->buf4; +} + +static void libsamplerate_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) { + struct impl_libsamplerate *u; + float *buf; + void *input, *output; + unsigned n_frames; + + assert(r); + assert(in); + assert(out); + assert(in->length); + assert(in->memblock); + assert(in->length % r->i_fz == 0); + assert(r->impl_data); + + u = r->impl_data; + + input = ((uint8_t*) in->memblock->data + in->index); + n_frames = in->length / r->i_fz; + assert(n_frames > 0); + + buf = convert_to_float(r, input, n_frames); + buf = remap_channels(r, buf, n_frames); + buf = resample(r, buf, &n_frames); + + if (n_frames) { + output = convert_from_float(r, buf, n_frames); + + if (output == input) { + /* Mm, no adjustment has been necessary, so let's return the original block */ + out->memblock = pa_memblock_ref(in->memblock); + out->index = in->index; + out->length = in->length; + } else { + float **p = NULL; + + out->length = n_frames * r->o_fz; + out->index = 0; + + if (output == u->buf1) { + p = &u->buf1; + u->buf1_samples = 0; + } else if (output == u->buf2) { + p = &u->buf2; + u->buf2_samples = 0; + } else if (output == u->buf3) { + p = &u->buf3; + u->buf3_samples = 0; + } else if (output == u->buf4) { + p = &u->buf4; + u->buf4_samples = 0; + } + + assert(p); + + /* Take the existing buffer and make it a memblock */ + out->memblock = pa_memblock_new_dynamic(*p, out->length, r->memblock_stat); + *p = NULL; + } + } else { + out->memblock = NULL; + out->index = out->length = 0; + } +} + +static void libsamplerate_update_input_rate(pa_resampler *r, uint32_t rate) { + struct impl_libsamplerate *u; + + assert(r); + assert(rate > 0); + assert(r->impl_data); + u = r->impl_data; + + if (!u->src_state) { + int err; + u->src_state = src_new(r->resample_method, r->o_ss.channels, &err); + assert(u->src_state); + } else { + int ret = src_set_ratio(u->src_state, (double) r->o_ss.rate / rate); + assert(ret == 0); + } +} + +static int libsamplerate_init(pa_resampler *r) { + struct impl_libsamplerate *u = NULL; + int err; + + r->impl_data = u = pa_xnew(struct impl_libsamplerate, 1); + + u->buf1 = u->buf2 = u->buf3 = u->buf4 = NULL; + u->buf1_samples = u->buf2_samples = u->buf3_samples = u->buf4_samples = 0; + + if (r->i_ss.format == PA_SAMPLE_FLOAT32NE) + u->to_float32ne_func = NULL; + else if (!(u->to_float32ne_func = pa_get_convert_to_float32ne_function(r->i_ss.format))) + goto fail; + + if (r->o_ss.format == PA_SAMPLE_FLOAT32NE) + u->from_float32ne_func = NULL; + else if (!(u->from_float32ne_func = pa_get_convert_from_float32ne_function(r->o_ss.format))) + goto fail; + + if (r->o_ss.rate == r->i_ss.rate) + u->src_state = NULL; + else if (!(u->src_state = src_new(r->resample_method, r->o_ss.channels, &err))) + goto fail; + + r->impl_free = libsamplerate_free; + r->impl_update_input_rate = libsamplerate_update_input_rate; + r->impl_run = libsamplerate_run; + + calc_map_table(r); + + return 0; + +fail: + pa_xfree(u); + return -1; +} + +/* Trivial implementation */ + +static void trivial_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) { + size_t fz; + unsigned n_frames; + struct impl_trivial *u; + + assert(r); + assert(in); + assert(out); + assert(r->impl_data); + + u = r->impl_data; + + fz = r->i_fz; + assert(fz == r->o_fz); + + n_frames = in->length/fz; + + if (r->i_ss.rate == r->o_ss.rate) { + + /* In case there's no diefference in sample types, do nothing */ + *out = *in; + pa_memblock_ref(out->memblock); + + u->o_counter += n_frames; + } else { + /* Do real resampling */ + size_t l; + unsigned o_index; + + /* The length of the new memory block rounded up */ + l = ((((n_frames+1) * r->o_ss.rate) / r->i_ss.rate) + 1) * fz; + + out->index = 0; + out->memblock = pa_memblock_new(l, r->memblock_stat); + + for (o_index = 0;; o_index++, u->o_counter++) { + unsigned j; + + j = (u->o_counter * r->i_ss.rate / r->o_ss.rate); + j = j > u->i_counter ? j - u->i_counter : 0; + + if (j >= n_frames) + break; + + assert(o_index*fz < out->memblock->length); + + memcpy((uint8_t*) out->memblock->data + fz*o_index, + (uint8_t*) in->memblock->data + in->index + fz*j, fz); + + } + + out->length = o_index*fz; + } + + u->i_counter += n_frames; + + /* Normalize counters */ + while (u->i_counter >= r->i_ss.rate) { + u->i_counter -= r->i_ss.rate; + assert(u->o_counter >= r->o_ss.rate); + u->o_counter -= r->o_ss.rate; + } +} + +static void trivial_free(pa_resampler *r) { + assert(r); + + pa_xfree(r->impl_data); +} + +static void trivial_update_input_rate(pa_resampler *r, uint32_t rate) { + struct impl_trivial *u; + + assert(r); + assert(rate > 0); + assert(r->impl_data); + + u = r->impl_data; + u->i_counter = 0; + u->o_counter = 0; +} + +static int trivial_init(pa_resampler*r) { + struct impl_trivial *u; + + assert(r); + assert(r->i_ss.format == r->o_ss.format); + assert(r->i_ss.channels == r->o_ss.channels); + + r->impl_data = u = pa_xnew(struct impl_trivial, 1); + u->o_counter = u->i_counter = 0; + + r->impl_run = trivial_run; + r->impl_free = trivial_free; + r->impl_update_input_rate = trivial_update_input_rate; + + return 0; +} + + -- cgit