| /*************************************************************************** |
| * __________ __ ___. |
| * 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 "buffer.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 */ |
| |
| 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 */ |
| }; |
| static struct tdspeed_state_s tdspeed_state; |
| |
| static int32_t *overlap_buffer[2] = { NULL, NULL }; |
| static int32_t *outbuf[2] = { NULL, NULL }; |
| |
| void tdspeed_init() |
| { |
| if (global_settings.timestretch_enabled) |
| { |
| /* Allocate buffers */ |
| if (overlap_buffer[0] == NULL) |
| overlap_buffer[0] = (int32_t *) buffer_alloc(FIXED_BUFSIZE * sizeof(int32_t)); |
| if (overlap_buffer[1] == NULL) |
| overlap_buffer[1] = (int32_t *) buffer_alloc(FIXED_BUFSIZE * sizeof(int32_t)); |
| if (outbuf[0] == NULL) |
| outbuf[0] = (int32_t *) buffer_alloc(TDSPEED_OUTBUFSIZE * sizeof(int32_t)); |
| if (outbuf[1] == NULL) |
| outbuf[1] = (int32_t *) buffer_alloc(TDSPEED_OUTBUFSIZE * sizeof(int32_t)); |
| } |
| } |
| |
| |
| 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 *curr, *prev, *dest[2], *d; |
| int32_t i, j, next_frame, prev_frame, shift, 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); |
| 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 */ |
| int64_t min_delta = ~(1ll << 63); /* most positive */ |
| shift = 0; |
| #define INC1 8 |
| #define INC2 32 |
| /* 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 (i = 0; i < st->shift_max; i += INC1) |
| { |
| int64_t delta = 0; |
| curr = buf_in[0] + next_frame + i; |
| prev = buf_in[0] + prev_frame; |
| for (j = 0; j < st->dst_step; j += INC2, curr += INC2, prev += INC2) |
| { |
| int32_t diff = *curr - *prev; |
| delta += (int64_t)diff * diff; |
| if (delta >= min_delta) |
| goto skip; |
| } |
| if (stereo) |
| { |
| curr = buf_in[1] +next_frame + i; |
| prev = buf_in[1] +prev_frame; |
| for (j = 0; j < st->dst_step; j += INC2, curr += INC2, prev += INC2) |
| { |
| int32_t diff = *curr - *prev; |
| delta += (int64_t)diff * diff; |
| if (delta >= min_delta) |
| goto skip; |
| } |
| } |
| min_delta = delta; |
| shift = i; |
| skip:; |
| } |
| |
| /* overlap fading-out previous frame with fading-in current frame */ |
| curr = buf_in[0] + next_frame + shift; |
| prev = buf_in[0] + prev_frame; |
| 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 (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 (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 */ |
| 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; |
| 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) |
| { |
| 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; |
| } |
| |