summaryrefslogtreecommitdiffstats
path: root/lib/rbcodec/dsp/tdspeed.c
diff options
context:
space:
mode:
Diffstat (limited to 'lib/rbcodec/dsp/tdspeed.c')
-rw-r--r--lib/rbcodec/dsp/tdspeed.c450
1 files changed, 450 insertions, 0 deletions
diff --git a/lib/rbcodec/dsp/tdspeed.c b/lib/rbcodec/dsp/tdspeed.c
new file mode 100644
index 0000000000..731be12621
--- /dev/null
+++ b/lib/rbcodec/dsp/tdspeed.c
@@ -0,0 +1,450 @@
+/***************************************************************************
+ * __________ __ ___.
+ * Open \______ \ ____ ____ | | _\_ |__ _______ ___
+ * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
+ * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
+ * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
+ * \/ \/ \/ \/ \/
+ * $Id$
+ *
+ * Copyright (C) 2006 by Nicolas Pitre <nico@cam.org>
+ * Copyright (C) 2006-2007 by Stéphane Doyon <s.doyon@videotron.ca>
+ *
+ * This program 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 Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+
+#include <inttypes.h>
+#include <stddef.h>
+#include <stdio.h>
+#include <string.h>
+#include "sound.h"
+#include "core_alloc.h"
+#include "system.h"
+#include "tdspeed.h"
+#include "settings.h"
+
+#define assert(cond)
+
+#define MIN_RATE 8000
+#define MAX_RATE 48000 /* double buffer for double rate */
+#define MINFREQ 100
+
+#define FIXED_BUFSIZE 3072 /* 48KHz factor 3.0 */
+
+static int32_t** dsp_src;
+static int handles[4];
+static int32_t *overlap_buffer[2] = { NULL, NULL };
+static int32_t *outbuf[2] = { NULL, NULL };
+
+static int move_callback(int handle, void* current, void* new)
+{
+ /* TODO */
+ (void)handle;
+ if (dsp_src)
+ {
+ int ch = (current == outbuf[0]) ? 0 : 1;
+ dsp_src[ch] = outbuf[ch] = new;
+ }
+ return BUFLIB_CB_OK;
+}
+
+static struct buflib_callbacks ops = {
+ .move_callback = move_callback,
+ .shrink_callback = NULL,
+};
+
+static int ovl_move_callback(int handle, void* current, void* new)
+{
+ /* TODO */
+ (void)handle;
+ if (dsp_src)
+ {
+ int ch = (current == overlap_buffer[0]) ? 0 : 1;
+ overlap_buffer[ch] = new;
+ }
+ return BUFLIB_CB_OK;
+}
+
+static struct buflib_callbacks ovl_ops = {
+ .move_callback = ovl_move_callback,
+ .shrink_callback = NULL,
+};
+
+
+static struct tdspeed_state_s
+{
+ bool stereo;
+ int32_t shift_max; /* maximum displacement on a frame */
+ int32_t src_step; /* source window pace */
+ int32_t dst_step; /* destination window pace */
+ int32_t dst_order; /* power of two for dst_step */
+ int32_t ovl_shift; /* overlap buffer frame shift */
+ int32_t ovl_size; /* overlap buffer used size */
+ int32_t ovl_space; /* overlap buffer size */
+ int32_t *ovl_buff[2]; /* overlap buffer */
+} tdspeed_state;
+
+void tdspeed_init(void)
+{
+ if (!global_settings.timestretch_enabled)
+ return;
+
+ /* Allocate buffers */
+ if (overlap_buffer[0] == NULL)
+ {
+ handles[0] = core_alloc_ex("tdspeed ovl left", FIXED_BUFSIZE * sizeof(int32_t), &ovl_ops);
+ overlap_buffer[0] = core_get_data(handles[0]);
+ }
+ if (overlap_buffer[1] == NULL)
+ {
+ handles[1] = core_alloc_ex("tdspeed ovl right", FIXED_BUFSIZE * sizeof(int32_t), &ovl_ops);
+ overlap_buffer[1] = core_get_data(handles[1]);
+ }
+ if (outbuf[0] == NULL)
+ {
+ handles[2] = core_alloc_ex("tdspeed left", TDSPEED_OUTBUFSIZE * sizeof(int32_t), &ops);
+ outbuf[0] = core_get_data(handles[2]);
+ }
+ if (outbuf[1] == NULL)
+ {
+ handles[3] = core_alloc_ex("tdspeed right", TDSPEED_OUTBUFSIZE * sizeof(int32_t), &ops);
+ outbuf[1] = core_get_data(handles[3]);
+ }
+}
+
+void tdspeed_finish(void)
+{
+ for(unsigned i = 0; i < ARRAYLEN(handles); i++)
+ {
+ if (handles[i] > 0)
+ {
+ core_free(handles[i]);
+ handles[i] = 0;
+ }
+ }
+ overlap_buffer[0] = overlap_buffer[1] = NULL;
+ outbuf[0] = outbuf[1] = NULL;
+}
+
+bool tdspeed_config(int samplerate, bool stereo, int32_t factor)
+{
+ struct tdspeed_state_s *st = &tdspeed_state;
+ int src_frame_sz;
+
+ /* Check buffers were allocated ok */
+ if (overlap_buffer[0] == NULL || overlap_buffer[1] == NULL)
+ return false;
+
+ if (outbuf[0] == NULL || outbuf[1] == NULL)
+ return false;
+
+ /* Check parameters */
+ if (factor == PITCH_SPEED_100)
+ return false;
+
+ if (samplerate < MIN_RATE || samplerate > MAX_RATE)
+ return false;
+
+ if (factor < STRETCH_MIN || factor > STRETCH_MAX)
+ return false;
+
+ st->stereo = stereo;
+ st->dst_step = samplerate / MINFREQ;
+
+ if (factor > PITCH_SPEED_100)
+ st->dst_step = st->dst_step * PITCH_SPEED_100 / factor;
+
+ st->dst_order = 1;
+
+ while (st->dst_step >>= 1)
+ st->dst_order++;
+
+ st->dst_step = (1 << st->dst_order);
+ st->src_step = st->dst_step * factor / PITCH_SPEED_100;
+ st->shift_max = (st->dst_step > st->src_step) ? st->dst_step : st->src_step;
+
+ src_frame_sz = st->shift_max + st->dst_step;
+
+ if (st->dst_step > st->src_step)
+ src_frame_sz += st->dst_step - st->src_step;
+
+ st->ovl_space = ((src_frame_sz - 2) / st->src_step) * st->src_step
+ + src_frame_sz;
+
+ if (st->src_step > st->dst_step)
+ st->ovl_space += 2*st->src_step - st->dst_step;
+
+ if (st->ovl_space > FIXED_BUFSIZE)
+ st->ovl_space = FIXED_BUFSIZE;
+
+ st->ovl_size = 0;
+ st->ovl_shift = 0;
+
+ st->ovl_buff[0] = overlap_buffer[0];
+
+ if (stereo)
+ st->ovl_buff[1] = overlap_buffer[1];
+ else
+ st->ovl_buff[1] = st->ovl_buff[0];
+
+ return true;
+}
+
+static int tdspeed_apply(int32_t *buf_out[2], int32_t *buf_in[2],
+ int data_len, int last, int out_size)
+/* data_len in samples */
+{
+ struct tdspeed_state_s *st = &tdspeed_state;
+ int32_t *dest[2];
+ int32_t next_frame, prev_frame, src_frame_sz;
+ bool stereo = buf_in[0] != buf_in[1];
+
+ assert(stereo == st->stereo);
+
+ src_frame_sz = st->shift_max + st->dst_step;
+
+ if (st->dst_step > st->src_step)
+ src_frame_sz += st->dst_step - st->src_step;
+
+ /* deal with overlap data first, if any */
+ if (st->ovl_size)
+ {
+ int32_t have, copy, steps;
+ have = st->ovl_size;
+
+ if (st->ovl_shift > 0)
+ have -= st->ovl_shift;
+
+ /* append just enough data to have all of the overlap buffer consumed */
+ steps = (have - 1) / st->src_step;
+ copy = steps * st->src_step + src_frame_sz - have;
+
+ if (copy < src_frame_sz - st->dst_step)
+ copy += st->src_step; /* one more step to allow for pregap data */
+
+ if (copy > data_len)
+ copy = data_len;
+
+ assert(st->ovl_size + copy <= FIXED_BUFSIZE);
+ memcpy(st->ovl_buff[0] + st->ovl_size, buf_in[0],
+ copy * sizeof(int32_t));
+
+ if (stereo)
+ memcpy(st->ovl_buff[1] + st->ovl_size, buf_in[1],
+ copy * sizeof(int32_t));
+
+ if (!last && have + copy < src_frame_sz)
+ {
+ /* still not enough to process at least one frame */
+ st->ovl_size += copy;
+ return 0;
+ }
+
+ /* recursively call ourselves to process the overlap buffer */
+ have = st->ovl_size;
+ st->ovl_size = 0;
+
+ if (copy == data_len)
+ {
+ assert(have + copy <= FIXED_BUFSIZE);
+ return tdspeed_apply(buf_out, st->ovl_buff, have+copy, last,
+ out_size);
+ }
+
+ assert(have + copy <= FIXED_BUFSIZE);
+ int i = tdspeed_apply(buf_out, st->ovl_buff, have+copy, -1, out_size);
+
+ dest[0] = buf_out[0] + i;
+ dest[1] = buf_out[1] + i;
+
+ /* readjust pointers to account for data already consumed */
+ next_frame = copy - src_frame_sz + st->src_step;
+ prev_frame = next_frame - st->ovl_shift;
+ }
+ else
+ {
+ dest[0] = buf_out[0];
+ dest[1] = buf_out[1];
+
+ next_frame = prev_frame = 0;
+
+ if (st->ovl_shift > 0)
+ next_frame += st->ovl_shift;
+ else
+ prev_frame += -st->ovl_shift;
+ }
+
+ st->ovl_shift = 0;
+
+ /* process all complete frames */
+ while (data_len - next_frame >= src_frame_sz)
+ {
+ /* find frame overlap by autocorelation */
+ int const INC1 = 8;
+ int const INC2 = 32;
+
+ int64_t min_delta = INT64_MAX; /* most positive */
+ int shift = 0;
+
+ /* Power of 2 of a 28bit number requires 56bits, can accumulate
+ 256times in a 64bit variable. */
+ assert(st->dst_step / INC2 <= 256);
+ assert(next_frame + st->shift_max - 1 + st->dst_step - 1 < data_len);
+ assert(prev_frame + st->dst_step - 1 < data_len);
+
+ for (int i = 0; i < st->shift_max; i += INC1)
+ {
+ int64_t delta = 0;
+
+ int32_t *curr = buf_in[0] + next_frame + i;
+ int32_t *prev = buf_in[0] + prev_frame;
+
+ for (int j = 0; j < st->dst_step; j += INC2, curr += INC2, prev += INC2)
+ {
+ int32_t diff = *curr - *prev;
+ delta += abs(diff);
+
+ if (delta >= min_delta)
+ goto skip;
+ }
+
+ if (stereo)
+ {
+ curr = buf_in[1] + next_frame + i;
+ prev = buf_in[1] + prev_frame;
+
+ for (int j = 0; j < st->dst_step; j += INC2, curr += INC2, prev += INC2)
+ {
+ int32_t diff = *curr - *prev;
+ delta += abs(diff);
+
+ if (delta >= min_delta)
+ goto skip;
+ }
+ }
+
+ min_delta = delta;
+ shift = i;
+skip:;
+ }
+
+ /* overlap fading-out previous frame with fading-in current frame */
+ int32_t *curr = buf_in[0] + next_frame + shift;
+ int32_t *prev = buf_in[0] + prev_frame;
+
+ int32_t *d = dest[0];
+
+ assert(next_frame + shift + st->dst_step - 1 < data_len);
+ assert(prev_frame + st->dst_step - 1 < data_len);
+ assert(dest[0] - buf_out[0] + st->dst_step - 1 < out_size);
+
+ for (int i = 0, j = st->dst_step; j; i++, j--)
+ {
+ *d++ = (*curr++ * (int64_t)i +
+ *prev++ * (int64_t)j) >> st->dst_order;
+ }
+
+ dest[0] = d;
+
+ if (stereo)
+ {
+ curr = buf_in[1] + next_frame + shift;
+ prev = buf_in[1] + prev_frame;
+
+ d = dest[1];
+
+ for (int i = 0, j = st->dst_step; j; i++, j--)
+ {
+ assert(d < buf_out[1] + out_size);
+
+ *d++ = (*curr++ * (int64_t)i +
+ *prev++ * (int64_t)j) >> st->dst_order;
+ }
+
+ dest[1] = d;
+ }
+
+ /* adjust pointers for next frame */
+ prev_frame = next_frame + shift + st->dst_step;
+ next_frame += st->src_step;
+
+ /* here next_frame - prev_frame = src_step - dst_step - shift */
+ assert(next_frame - prev_frame == st->src_step - st->dst_step - shift);
+ }
+
+ /* now deal with remaining partial frames */
+ if (last == -1)
+ {
+ /* special overlap buffer processing: remember frame shift only */
+ st->ovl_shift = next_frame - prev_frame;
+ }
+ else if (last != 0)
+ {
+ /* last call: purge all remaining data to output buffer */
+ int i = data_len - prev_frame;
+
+ assert(dest[0] + i <= buf_out[0] + out_size);
+ memcpy(dest[0], buf_in[0] + prev_frame, i * sizeof(int32_t));
+
+ dest[0] += i;
+
+ if (stereo)
+ {
+ assert(dest[1] + i <= buf_out[1] + out_size);
+ memcpy(dest[1], buf_in[1] + prev_frame, i * sizeof(int32_t));
+ dest[1] += i;
+ }
+ }
+ else
+ {
+ /* preserve remaining data + needed overlap data for next call */
+ st->ovl_shift = next_frame - prev_frame;
+ int i = (st->ovl_shift < 0) ? next_frame : prev_frame;
+ st->ovl_size = data_len - i;
+
+ assert(st->ovl_size <= FIXED_BUFSIZE);
+ memcpy(st->ovl_buff[0], buf_in[0] + i, st->ovl_size * sizeof(int32_t));
+
+ if (stereo)
+ memcpy(st->ovl_buff[1], buf_in[1] + i, st->ovl_size * sizeof(int32_t));
+ }
+
+ return dest[0] - buf_out[0];
+}
+
+long tdspeed_est_output_size()
+{
+ return TDSPEED_OUTBUFSIZE;
+}
+
+long tdspeed_est_input_size(long size)
+{
+ struct tdspeed_state_s *st = &tdspeed_state;
+
+ size = (size - st->ovl_size) * st->src_step / st->dst_step;
+
+ if (size < 0)
+ size = 0;
+
+ return size;
+}
+
+int tdspeed_doit(int32_t *src[], int count)
+{
+ dsp_src = src;
+ count = tdspeed_apply( (int32_t *[2]) { outbuf[0], outbuf[1] },
+ src, count, 0, TDSPEED_OUTBUFSIZE);
+
+ src[0] = outbuf[0];
+ src[1] = outbuf[1];
+
+ return count;
+}
+