From a64e85acf96bc0c55363fe55c9e9116aef2a8584 Mon Sep 17 00:00:00 2001 From: Lennart Poettering Date: Sat, 12 May 2007 23:38:38 +0000 Subject: resampling works git-svn-id: file:///home/lennart/svn/public/libsydney/trunk@4 9ba3c220-e4d3-45a2-8aa3-73fcc9aff6ce --- converter.c | 739 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 739 insertions(+) create mode 100644 converter.c (limited to 'converter.c') diff --git a/converter.c b/converter.c new file mode 100644 index 0000000..799349c --- /dev/null +++ b/converter.c @@ -0,0 +1,739 @@ +#include +#include + +#include "speex/speex_resampler.h" + +#include "converter.h" +#include "sydney.h" +#include "macro.h" +#include "common.h" +#include "format.h" +#include "volscale.h" +#include "byteswap.h" +#include "zero.h" +#include "add.h" +#include "bbuffer.h" +#include "continued-fraction.h" +#include "malloc.h" +#include "resample.h" + +/* Steps: byteswap -> convert -> volscale -> remap -> resample -> convert -> byteswap -> interleave */ + +/* Sample formats we know to process natively */ +static int native_pcm_format_process(sa_pcm_format_t f) { + return + f == SA_PCM_FORMAT_U8 || + f == SA_PCM_FORMAT_S16_NE || + f == SA_PCM_FORMAT_S32_NE || + f == SA_PCM_FORMAT_FLOAT32_NE; +} + +/* Sample formats we know to resample natively */ +static int native_pcm_format_resample(sa_pcm_format_t f) { + return + f == SA_PCM_FORMAT_S16_NE || + f == SA_PCM_FORMAT_FLOAT32_NE; +} + +static sa_pcm_format_t byteswap_fix(sa_pcm_format_t f) { + + switch (f) { + case SA_PCM_FORMAT_U8: + case SA_PCM_FORMAT_ULAW: + case SA_PCM_FORMAT_ALAW: + return f; + + case SA_PCM_FORMAT_S16_NE: + case SA_PCM_FORMAT_S16_RE: + return SA_PCM_FORMAT_S16_NE; + + case SA_PCM_FORMAT_S24_NE: + case SA_PCM_FORMAT_S24_RE: + return SA_PCM_FORMAT_S24_NE; + + case SA_PCM_FORMAT_S32_NE: + case SA_PCM_FORMAT_S32_RE: + return SA_PCM_FORMAT_S32_NE; + + case SA_PCM_FORMAT_FLOAT32_NE: + case SA_PCM_FORMAT_FLOAT32_RE: + return SA_PCM_FORMAT_FLOAT32_NE; + + case SA_PCM_FORMAT_MAX: + ; + } + + sa_assert_not_reached(); +} + +static sa_pcm_format_t get_work_format(sa_pcm_format_t a, sa_pcm_format_t b) { + + switch (a) { + case SA_PCM_FORMAT_U8: + case SA_PCM_FORMAT_ULAW: + case SA_PCM_FORMAT_ALAW: + + switch (b) { + case SA_PCM_FORMAT_U8: + return SA_PCM_FORMAT_U8; + + case SA_PCM_FORMAT_ULAW: + case SA_PCM_FORMAT_ALAW: + case SA_PCM_FORMAT_S16_LE: + case SA_PCM_FORMAT_S16_BE: + return SA_PCM_FORMAT_S16_NE; + + case SA_PCM_FORMAT_S24_LE: + case SA_PCM_FORMAT_S24_BE: + case SA_PCM_FORMAT_S32_LE: + case SA_PCM_FORMAT_S32_BE: + return SA_PCM_FORMAT_S32_NE; + + case SA_PCM_FORMAT_FLOAT32_LE: + case SA_PCM_FORMAT_FLOAT32_BE: + return SA_PCM_FORMAT_FLOAT32_NE; + + case SA_PCM_FORMAT_MAX: + ; + } + + break; + + case SA_PCM_FORMAT_S16_LE: + case SA_PCM_FORMAT_S16_BE: + + switch (b) { + case SA_PCM_FORMAT_U8: + case SA_PCM_FORMAT_ULAW: + case SA_PCM_FORMAT_ALAW: + case SA_PCM_FORMAT_S16_LE: + case SA_PCM_FORMAT_S16_BE: + return SA_PCM_FORMAT_S16_NE; + + case SA_PCM_FORMAT_S24_LE: + case SA_PCM_FORMAT_S24_BE: + case SA_PCM_FORMAT_S32_LE: + case SA_PCM_FORMAT_S32_BE: + return SA_PCM_FORMAT_S32_NE; + + case SA_PCM_FORMAT_FLOAT32_LE: + case SA_PCM_FORMAT_FLOAT32_BE: + return SA_PCM_FORMAT_FLOAT32_NE; + + case SA_PCM_FORMAT_MAX: + ; + } + break; + + case SA_PCM_FORMAT_S24_LE: + case SA_PCM_FORMAT_S24_BE: + case SA_PCM_FORMAT_S32_LE: + case SA_PCM_FORMAT_S32_BE: + + switch (b) { + case SA_PCM_FORMAT_U8: + case SA_PCM_FORMAT_ULAW: + case SA_PCM_FORMAT_ALAW: + case SA_PCM_FORMAT_S16_LE: + case SA_PCM_FORMAT_S16_BE: + case SA_PCM_FORMAT_S24_LE: + case SA_PCM_FORMAT_S24_BE: + case SA_PCM_FORMAT_S32_LE: + case SA_PCM_FORMAT_S32_BE: + return SA_PCM_FORMAT_S32_NE; + + case SA_PCM_FORMAT_FLOAT32_LE: + case SA_PCM_FORMAT_FLOAT32_BE: + return SA_PCM_FORMAT_FLOAT32_NE; + + case SA_PCM_FORMAT_MAX: + ; + } + break; + + case SA_PCM_FORMAT_FLOAT32_LE: + case SA_PCM_FORMAT_FLOAT32_BE: + return SA_PCM_FORMAT_FLOAT32_NE; + + case SA_PCM_FORMAT_MAX: + ; + } + + sa_assert_not_reached(); +} + +static sa_pcm_format_t fix_work_format_for_resample(sa_pcm_format_t f) { + + switch (f) { + case SA_PCM_FORMAT_U8: + case SA_PCM_FORMAT_S16_NE: + return SA_PCM_FORMAT_S16_NE; + break; + + case SA_PCM_FORMAT_S32_NE: + case SA_PCM_FORMAT_FLOAT32_NE: + return SA_PCM_FORMAT_FLOAT32_NE; + break; + + default: + ; + } + + sa_assert_not_reached(); +} + +int converter_init( + converter_t *c, + const pcm_attrs_t *from, + const pcm_attrs_t *to, + int dynamic_rate_enabled) { + + unsigned u, t; + int resample_required; + + sa_assert(c); + sa_assert(from); + sa_assert(to); + + memset(c, 0, sizeof(*c)); + + c->from_pcm_format = from->format; + c->to_pcm_format = to->format; + + c->from_rate = from->rate; + c->to_rate = to->rate; + + c->from_nchannels = from->nchannels; + c->to_nchannels = to->nchannels; + + if (!(c->channel_map_table = sa_new(int, (from->nchannels+1) * to->nchannels))) + goto fail; + + for (u = 0; u < to->nchannels; u++) { + unsigned k = 0; + + for (t = 0; t < from->nchannels; t++) { + + if (from->channel_map[u] == to->channel_map[t]) { + if (u != t) + c->remap_required = 1; + + c->channel_map_table[u * (from->nchannels+1) + k++] += t; + } + } + + if (k > 1) + c->sum_required = 1; + + c->channel_map_table[k] = (unsigned) -1; + } + + if (c->from_nchannels != c->to_nchannels) + c->remap_required = 1; + + resample_required = + from->rate != to->rate || + dynamic_rate_enabled; + + c->work_pcm_format = get_work_format(from->format, to->format); + if (resample_required) + c->work_pcm_format = fix_work_format_for_resample(c->work_pcm_format); + + sa_assert(native_pcm_format_process(c->work_pcm_format)); + sa_assert(!resample_required || native_pcm_format_resample(c->work_pcm_format)); + + c->from_sample_size = get_pcm_sample_size(c->from_pcm_format); + c->work_sample_size = get_pcm_sample_size(c->work_pcm_format); + c->to_sample_size = get_pcm_sample_size(c->to_pcm_format); + + /* Get function pointers */ + c->pre_byteswap_func = get_byteswap_func(from->format); + + if (byteswap_fix(from->format) != c->work_pcm_format) { + c->pre_format_func = get_format_func(byteswap_fix(from->format), c->work_pcm_format); + sa_assert(c->pre_format_func); + } else + c->pre_format_func = NULL; + + c->volscale_func = get_volscale_func(c->work_pcm_format); + sa_assert(c->volscale_func); + + c->zero_func = get_zero_func(c->work_pcm_format); + sa_assert(c->zero_func); + + c->add_func = get_add_func(c->work_pcm_format); + sa_assert(c->add_func); + + if (resample_required) { + c->resample_func = get_resample_func(c->work_pcm_format); + sa_assert(c->resample_func); + } else + c->resample_func = NULL; + + if (c->work_pcm_format != byteswap_fix(to->format)) { + c->post_format_func = get_format_func(c->work_pcm_format, byteswap_fix(to->format)); + sa_assert(c->post_format_func); + } else + c->post_format_func = NULL; + + c->post_byteswap_func = get_byteswap_func(to->format); + + c->interleave_func = get_interleave_func(to->format); + sa_assert(c->interleave_func); + + /* Initialize resampler */ + + if (!(c->speex = speex_resampler_init(c->to_nchannels, c->from_rate, c->to_rate, SPEEX_RESAMPLER_QUALITY_DEFAULT, NULL))) + goto fail; + + /* Initialize processing variables */ + + if (!(c->from_process_data = sa_new0(void*, c->from_nchannels))) + goto fail; + + if (!(c->from_stride = sa_new0(size_t, c->from_nchannels))) + goto fail; + + if (!(c->to_process_data = sa_new0(void*, c->to_nchannels))) + goto fail; + + if (!(c->to_stride = sa_new0(size_t, c->to_nchannels))) + goto fail; + + /* Initialize volume stuff */ + if (!(c->volume_factor = sa_new(int32_t, c->from_nchannels))) + goto fail; + + if (!(c->volume_divisor = sa_new(int32_t, c->from_nchannels))) + goto fail; + + c->no_volume = 1; + + /* Initialize bounce buffers */ + + if (bbuffer_init(&c->bb_pre_byteswap, c->from_nchannels, c->from_sample_size) < 0) + goto fail; + if (bbuffer_init(&c->bb_pre_format, c->from_nchannels, c->work_sample_size) < 0) + goto fail; + if (bbuffer_init(&c->bb_volscale, c->from_nchannels, c->work_sample_size) < 0) + goto fail; + if (bbuffer_init(&c->bb_remap, c->to_nchannels, c->work_sample_size) < 0) + goto fail; + if (bbuffer_init(&c->bb_resample, c->to_nchannels, c->work_sample_size) < 0) + goto fail; + if (bbuffer_init(&c->bb_post_format, c->to_nchannels, c->to_sample_size) < 0) + goto fail; + if (bbuffer_init(&c->bb_post_byteswap, c->to_nchannels, c->to_sample_size) < 0) + goto fail; + if (bbuffer_init(&c->bb_interleave, c->to_nchannels, c->to_sample_size) < 0) + goto fail; + if (bbuffer_init(&c->bb_tmp, 1, 1) < 0) + goto fail; + + return 0; + +fail: + converter_done(c); + + return SA_ERROR_OOM; +} + +void converter_done(converter_t *c) { + sa_assert(c); + + sa_free(c->channel_map_table); + + sa_free(c->from_process_data); + sa_free(c->to_process_data); + sa_free(c->from_stride); + sa_free(c->to_stride); + + bbuffer_done(&c->bb_pre_byteswap); + bbuffer_done(&c->bb_pre_format); + bbuffer_done(&c->bb_volscale); + bbuffer_done(&c->bb_remap); + bbuffer_done(&c->bb_resample); + bbuffer_done(&c->bb_post_format); + bbuffer_done(&c->bb_post_byteswap); + bbuffer_done(&c->bb_interleave); + bbuffer_done(&c->bb_tmp); + + if (c->speex) + speex_resampler_destroy(c->speex); + + sa_free(c->zero_buffer); + + sa_free(c->volume_divisor); + sa_free(c->volume_factor); + memset(c, 0, sizeof(*c)); +} + +static void* get_zero_buffer(converter_t *c, size_t size) { + void *b; + + sa_assert(c); + sa_assert(size > 0); + + if (c->zero_buffer && c->zero_size >= size) + return c->zero_buffer; + + sa_free(c->zero_buffer); + + if (!(c->zero_buffer = sa_malloc(size))) + return NULL; + + c->zero_func(b, c->work_sample_size, size); + return c->zero_func; +} + +int converter_go( + converter_t *c, + const void *const src[], const size_t sstr[], int sinterleave, + const void **dst[], size_t *dstr[], int dinterleave, + size_t *size) { + + const size_t* stride; + void** process_data; + int is_bounce; + int interleave; + unsigned i; + + sa_assert(c); + + is_bounce = 0; + stride = sstr; + process_data = (void**) src; + interleave = !!sinterleave; + dinterleave = !!dinterleave; + + if (c->no_volume && + !c->remap_required && + !c->resample_func && + c->from_pcm_format == c->to_pcm_format) { + + /* We can shortcut this, since we don't need to do any real work.*/ + + goto do_interleave; + } + + if (c->pre_byteswap_func) { + size_t k; + + k = dinterleave ? c->from_sample_size*c->from_nchannels : c->from_sample_size; + + for (i = 0; i < c->from_nchannels; i++) { + void *b; + + if (!(b = bbuffer_get(&c->bb_pre_byteswap, i, *size, dinterleave))) + return SA_ERROR_OOM; + + c->pre_byteswap_func(b, k, process_data[i], stride[i], *size); + c->from_process_data[i] = b; + c->from_stride[i] = k; + } + + process_data = c->from_process_data; + stride = c->from_stride; + interleave = dinterleave; + is_bounce = 1; + } + + if (c->pre_format_func) { + + if (is_bounce && c->from_sample_size == c->to_sample_size) { + + /* The data to process is already in a bounce buffer, we + * can do this in-place */ + + for (i = 0; i < c->from_nchannels; i++) + c->pre_format_func(&c->bb_tmp, process_data[i], stride[i], process_data[i], stride[i], *size); + + } else { + size_t k, new_size; + + new_size = *size / c->from_sample_size * c->work_sample_size; + k = dinterleave ? c->work_sample_size*c->from_nchannels : c->work_sample_size; + + for (i = 0; i < c->from_nchannels; i++) { + void *b; + + if (!(b = bbuffer_get(&c->bb_pre_format, i, new_size, dinterleave))) + return SA_ERROR_OOM; + + c->pre_format_func(&c->bb_tmp, b, k, process_data[i], stride[i], *size); + c->from_process_data[i] = b; + c->from_stride[i] = k; + } + + process_data = c->from_process_data; + stride = c->from_stride; + *size = new_size; + interleave = dinterleave; + is_bounce = 1; + } + } + + if (!c->no_volume) { + + sa_assert(c->volscale_func); + + if (is_bounce) { + + /* The data to process is already in a bounce buffer, we + * can do this in-place */ + + for (i = 0; i < c->from_nchannels; i++); + c->volscale_func(process_data[i], stride[i], process_data[i], stride[i], c->volume_factor[i], c->volume_divisor[i], *size); + + } else { + size_t k; + + k = dinterleave ? c->work_sample_size*c->from_nchannels : c->work_sample_size; + + for (i = 0; i < c->from_nchannels; i++) { + void *b; + + if (!(b = bbuffer_get(&c->bb_volscale, i, *size, dinterleave))) + return SA_ERROR_OOM; + + c->volscale_func(b, k, process_data[i], stride[i], c->volume_factor[i], c->volume_divisor[i], *size); + c->from_process_data[i] = b; + c->from_stride[i] = k; + } + + process_data = c->from_process_data; + stride = c->from_stride; + interleave = dinterleave; + is_bounce = 1; + } + } + + if (c->remap_required) { + size_t k; + int need_proper_interleave = 0; + + k = dinterleave ? c->work_sample_size*c->to_nchannels : c->work_sample_size; + + for (i = 0; i < c->to_nchannels; i++) { + void *b; + int *p = &c->channel_map_table[i * (c->from_nchannels+1)]; + + if (p[0] == -1) { + /* We have to write silence to this channel */ + + if (!(b = get_zero_buffer(c, *size))) + return SA_ERROR_OOM; + + c->to_process_data[i] = b; + c->to_stride[i] = c->work_sample_size; + + need_proper_interleave = 1; + } else if (p[1] == -1) { + /* Just one channel, nothing to mix */ + + c->to_process_data[i] = process_data[p[0]]; + c->to_stride[i] = stride[p[0]]; + + need_proper_interleave = 1; + } else { + int j; + + /* We have to mix two or more channels */ + + if (!(b = bbuffer_get(&c->bb_remap, i, *size, dinterleave))) + return SA_ERROR_OOM; + + c->add_func(b, k, process_data[p[0]], stride[p[0]], process_data[p[1]], stride[p[1]], *size); + + for (j = 2; p[j] != -1; j++) + c->add_func(b, k, b, k, process_data[p[j]], stride[p[j]], *size); + + c->to_process_data[i] = b; + c->to_stride[i] = k; + } + } + + process_data = c->to_process_data; + stride = c->to_stride; + interleave = need_proper_interleave ? -1 : dinterleave; + is_bounce = 1; + } + + if (c->resample_func) { + size_t k; + size_t new_size; + + k = dinterleave ? c->work_sample_size*c->to_nchannels : c->work_sample_size; + + new_size = (size_t) (((((uint64_t) *size+c->work_sample_size-1)/c->work_sample_size)*c->to_rate)/c->from_rate+1)*c->work_sample_size; /* FIXME */ + + for (i = 0; i < c->to_nchannels; i++) { + void *b; + + if (!(b = bbuffer_get(&c->bb_resample, i, new_size, dinterleave))) + return SA_ERROR_OOM; + + c->resample_func(c->speex, i, b, k, process_data[i], stride[i], *size, &new_size); + c->to_process_data[i] = b; + c->to_stride[i] = k; + } + + process_data = c->to_process_data; + stride = c->to_stride; + *size = new_size; + interleave = dinterleave; + is_bounce = 1; + } + + if (c->post_format_func) { + + if (is_bounce && c->work_sample_size == c->to_sample_size) { + + /* The data to process is already in a bounce buffer, we + * can do this in-place */ + + for (i = 0; i < c->to_nchannels; i++) + c->post_format_func(&c->bb_tmp, process_data[i], stride[i], process_data[i], stride[i], *size); + + } else { + size_t k, new_size; + + k = dinterleave ? c->to_sample_size*c->to_nchannels : c->to_sample_size; + new_size = *size / c->work_sample_size * c->to_sample_size; + + for (i = 0; i < c->to_nchannels; i++) { + void *b; + + if (!(b = bbuffer_get(&c->bb_post_format, i, new_size, dinterleave))) + return SA_ERROR_OOM; + + c->post_format_func(&c->bb_tmp, b, k, process_data[i], stride[i], *size); + c->to_process_data[i] = b; + c->to_stride[i] = k; + } + + process_data = c->to_process_data; + stride = c->to_stride; + *size = new_size; + interleave = dinterleave; + is_bounce = 1; + } + } + + if (c->post_byteswap_func) { + + if (is_bounce) { + + /* The data to process is already in a bounce buffer, we + * can do this in-place */ + + for (i = 0; i < c->to_nchannels; i++) + c->post_byteswap_func(process_data[i], stride[i], process_data[i], stride[i], *size); + + } else { + size_t k; + + k = dinterleave ? c->to_sample_size*c->to_nchannels : c->to_sample_size; + + for (i = 0; i < c->to_nchannels; i++) { + void *b; + + if (!(b = bbuffer_get(&c->bb_post_byteswap, i, *size, dinterleave))) + return SA_ERROR_OOM; + + c->post_byteswap_func(b, k, process_data[i], stride[i], *size); + + c->to_process_data[i] = b; + c->to_stride[i] = k; + } + + process_data = c->to_process_data; + stride = c->to_stride; + interleave = dinterleave; + is_bounce = 1; + } + } + +do_interleave: + + if (interleave != dinterleave) { + size_t k; + + k = dinterleave ? c->to_sample_size*c->to_nchannels : c->to_sample_size; + + for (i = 0; i < c->to_nchannels; i++) { + void *b; + + if (!(b = bbuffer_get(&c->bb_interleave, i, *size, dinterleave))) + return SA_ERROR_OOM; + + c->interleave_func(b, k, process_data[i], stride[i], *size); + + c->to_process_data[i] = b; + c->to_stride[i] = k; + } + + process_data = c->to_process_data; + stride = c->to_stride; + interleave = dinterleave; + is_bounce = 1; + } + + *dstr = stride; + *dst = process_data; + + return SA_SUCCESS; +} + +void converter_set_volume(converter_t *c, int vol[]) { + unsigned i; + int no_volume = 1; + + sa_assert(c); + + for (i = 0; i < c->from_nchannels; i++) { + int num, denom; + + if (vol[i] == 0) { + c->volume_factor[i] = 1; + c->volume_divisor[1] = 1; + } else { + float f = powf(10.0, vol[i] * 10); + + continued_fraction(f, 0x7FFF, &num, &denom); + + if (num != 1 || denom != 1) + no_volume = 0; + + c->volume_factor[i] = (int32_t) num; + c->volume_divisor[i] = (int32_t) denom; + } + } + +} + +int converter_go_interleaved( + converter_t *c, + const void *const data, + const void **dst[], size_t *dstr[], int dinterleave, + size_t *size) { + + unsigned i; + const uint8_t *d = data; + unsigned stride = c->from_nchannels * c->from_sample_size; + + for (i = 0; i < c->from_nchannels; i++) { + c->from_process_data[i] = (void*) data; + d += c->from_sample_size; + c->from_stride[i] = stride; + } + + return converter_go(c, c->from_process_data, c->from_stride, 1, dst, dstr, dinterleave, size); +} + +void converter_set_ratio(converter_t *c, unsigned rate1, unsigned rate2) { + assert(c); + assert(c->speex); + + speex_resampler_set_rate(c->speex, rate1, rate2); +} -- cgit