apps/tdspeed.c

   1 /***************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   9  *
  10  * Copyright (C) 2006 by Nicolas Pitre <nico@cam.org>
  11  * Copyright (C) 2006-2007 by Stéphane Doyon <s.doyon@videotron.ca>
  12  *
  13  * This program is free software; you can redistribute it and/or
  14  * modify it under the terms of the GNU General Public License
  15  * as published by the Free Software Foundation; either version 2
  16  * of the License, or (at your option) any later version.
  17  *
  18  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
  19  * KIND, either express or implied.
  20  *
  21  ****************************************************************************/
  22
  23 #include <inttypes.h>
  24 #include <stddef.h>
  25 #include <stdio.h>
  26 #include <string.h>
  27 #include "sound.h"
  28 #include "core_alloc.h"
  29 #include "system.h"
  30 #include "tdspeed.h"
  31 #include "settings.h"
  32
  33 #define assert(cond)
  34
  35 #define MIN_RATE 8000
  36 #define MAX_RATE 48000 /* double buffer for double rate */
  37 #define MINFREQ 100
  38
  39 #define FIXED_BUFSIZE 3072 /* 48KHz factor 3.0 */
  40
  41 static int32_t *overlap_buffer[2] = { NULL, NULL };
  42 static int32_t *outbuf[2] = { NULL, NULL };
  43
  44 struct tdspeed_state_s
  45 {
  46     bool stereo;
  47     int32_t shift_max;      /* maximum displacement on a frame */
  48     int32_t src_step;       /* source window pace */
  49     int32_t dst_step;       /* destination window pace */
  50     int32_t dst_order;      /* power of two for dst_step */
  51     int32_t ovl_shift;      /* overlap buffer frame shift */
  52     int32_t ovl_size;       /* overlap buffer used size */
  53     int32_t ovl_space;      /* overlap buffer size */
  54     int32_t *ovl_buff[2];   /* overlap buffer */
  55 };
  56 static struct tdspeed_state_s tdspeed_state;
  57
  58 void tdspeed_init()
  59 {
  60     if (global_settings.timestretch_enabled)
  61     {
  62         /* Allocate buffers */
  63         int handle;
  64         if (overlap_buffer[0] == NULL)
  65         {
  66             handle = core_alloc("tdspeed ovl left", FIXED_BUFSIZE * sizeof(int32_t));
  67             overlap_buffer[0] = core_get_data(handle);
  68         }
  69         if (overlap_buffer[1] == NULL)
  70         {
  71             handle = core_alloc("tdspeed ovl right", FIXED_BUFSIZE * sizeof(int32_t));
  72             overlap_buffer[1] = core_get_data(handle);
  73         }
  74         if (outbuf[0] == NULL)
  75         {
  76             handle = core_alloc("tdspeed left", TDSPEED_OUTBUFSIZE * sizeof(int32_t));
  77             outbuf[0] = core_get_data(handle);
  78         }
  79         if (outbuf[1] == NULL)
  80         {
  81             handle = core_alloc("tdspeed right", TDSPEED_OUTBUFSIZE * sizeof(int32_t));
  82             outbuf[1] = core_get_data(handle);
  83         }
  84     }
  85 }
  86
  87
  88 bool tdspeed_config(int samplerate, bool stereo, int32_t factor)
  89 {
  90     struct tdspeed_state_s *st = &tdspeed_state;
  91     int src_frame_sz;
  92
  93     /* Check buffers were allocated ok */
  94     if (overlap_buffer[0] == NULL || overlap_buffer[1] == NULL)
  95         return false;
  96     if (outbuf[0] == NULL || outbuf[1] == NULL)
  97         return false;
  98
  99     /* Check parameters */
 100     if (factor == PITCH_SPEED_100)
 101         return false;
 102     if (samplerate < MIN_RATE || samplerate > MAX_RATE)
 103         return false;
 104     if (factor < STRETCH_MIN || factor > STRETCH_MAX)
 105         return false;
 106
 107     st->stereo = stereo;
 108     st->dst_step = samplerate / MINFREQ;
 109
 110     if (factor > PITCH_SPEED_100)
 111         st->dst_step = st->dst_step * PITCH_SPEED_100 / factor;
 112     st->dst_order = 1;
 113
 114     while (st->dst_step >>= 1)
 115         st->dst_order++;
 116     st->dst_step = (1 << st->dst_order);
 117     st->src_step = st->dst_step * factor / PITCH_SPEED_100;
 118     st->shift_max = (st->dst_step > st->src_step) ? st->dst_step : st->src_step;
 119
 120     src_frame_sz = st->shift_max + st->dst_step;
 121     if (st->dst_step > st->src_step)
 122         src_frame_sz += st->dst_step - st->src_step;
 123     st->ovl_space = ((src_frame_sz - 2)/st->src_step) * st->src_step
 124         + src_frame_sz;
 125     if (st->src_step > st->dst_step)
 126         st->ovl_space += 2*st->src_step - st->dst_step;
 127
 128     if (st->ovl_space > FIXED_BUFSIZE)
 129         st->ovl_space = FIXED_BUFSIZE;
 130
 131     st->ovl_size = 0;
 132     st->ovl_shift = 0;
 133
 134     st->ovl_buff[0] = overlap_buffer[0];
 135     if (stereo)
 136         st->ovl_buff[1] = overlap_buffer[1];
 137     else
 138         st->ovl_buff[1] = st->ovl_buff[0];
 139
 140     return true;
 141 }
 142
 143 static int tdspeed_apply(int32_t *buf_out[2], int32_t *buf_in[2],
 144                          int data_len, int last, int out_size)
 145 /* data_len in samples */
 146 {
 147     struct tdspeed_state_s *st = &tdspeed_state;
 148     int32_t *curr, *prev, *dest[2], *d;
 149     int32_t i, j, next_frame, prev_frame, shift, src_frame_sz;
 150     bool stereo = buf_in[0] != buf_in[1];
 151     assert(stereo == st->stereo);
 152
 153     src_frame_sz = st->shift_max + st->dst_step;
 154     if (st->dst_step > st->src_step)
 155         src_frame_sz += st->dst_step - st->src_step;
 156
 157     /* deal with overlap data first, if any */
 158     if (st->ovl_size)
 159     {
 160         int32_t have, copy, steps;
 161         have = st->ovl_size;
 162         if (st->ovl_shift > 0)
 163             have -= st->ovl_shift;
 164         /* append just enough data to have all of the overlap buffer consumed */
 165         steps = (have - 1) / st->src_step;
 166         copy = steps * st->src_step + src_frame_sz - have;
 167         if (copy < src_frame_sz - st->dst_step)
 168             copy += st->src_step;  /* one more step to allow for pregap data */
 169         if (copy > data_len) copy = data_len;
 170         assert(st->ovl_size +copy <= FIXED_BUFSIZE);
 171         memcpy(st->ovl_buff[0] + st->ovl_size, buf_in[0],
 172                copy * sizeof(int32_t));
 173         if (stereo)
 174             memcpy(st->ovl_buff[1] + st->ovl_size, buf_in[1],
 175                    copy * sizeof(int32_t));
 176         if (!last && have + copy < src_frame_sz)
 177         {
 178             /* still not enough to process at least one frame */
 179             st->ovl_size += copy;
 180             return 0;
 181         }
 182
 183         /* recursively call ourselves to process the overlap buffer */
 184         have = st->ovl_size;
 185         st->ovl_size = 0;
 186         if (copy == data_len)
 187         {
 188             assert( (have+copy) <= FIXED_BUFSIZE);
 189             return tdspeed_apply(buf_out, st->ovl_buff, have+copy, last,
 190                                out_size);
 191         }
 192         assert( (have+copy) <= FIXED_BUFSIZE);
 193         i = tdspeed_apply(buf_out, st->ovl_buff, have+copy, -1, out_size);
 194         dest[0] = buf_out[0] + i;
 195         dest[1] = buf_out[1] + i;
 196
 197         /* readjust pointers to account for data already consumed */
 198         next_frame = copy - src_frame_sz + st->src_step;
 199         prev_frame = next_frame - st->ovl_shift;
 200     }
 201     else
 202     {
 203         dest[0] = buf_out[0];
 204         dest[1] = buf_out[1];
 205         next_frame = prev_frame = 0;
 206         if (st->ovl_shift > 0)
 207             next_frame += st->ovl_shift;
 208         else
 209           prev_frame += -st->ovl_shift;
 210     }
 211     st->ovl_shift = 0;
 212
 213     /* process all complete frames */
 214     while (data_len - next_frame >= src_frame_sz)
 215     {
 216         /* find frame overlap by autocorelation */
 217         int64_t min_delta = ~(1ll << 63);  /* most positive */
 218         shift = 0;
 219 #define INC1 8
 220 #define INC2 32
 221         /* Power of 2 of a 28bit number requires 56bits, can accumulate
 222            256times in a 64bit variable. */
 223         assert(st->dst_step / INC2 <= 256);
 224         assert(next_frame + st->shift_max - 1 + st->dst_step-1 < data_len);
 225         assert(prev_frame + st->dst_step - 1 < data_len);
 226         for (i = 0; i < st->shift_max; i += INC1)
 227         {
 228             int64_t delta = 0;
 229             curr = buf_in[0] + next_frame + i;
 230             prev = buf_in[0] + prev_frame;
 231             for (j = 0; j < st->dst_step; j += INC2, curr += INC2, prev += INC2)
 232             {
 233                 int32_t diff = *curr - *prev;
 234                 delta += (int64_t)diff * diff;
 235                 if (delta >= min_delta)
 236                     goto skip;
 237             }
 238             if (stereo)
 239             {
 240                 curr = buf_in[1] +next_frame + i;
 241                 prev = buf_in[1] +prev_frame;
 242                 for (j = 0; j < st->dst_step; j += INC2, curr += INC2, prev += INC2)
 243                 {
 244                     int32_t diff = *curr - *prev;
 245                     delta += (int64_t)diff * diff;
 246                     if (delta >= min_delta)
 247                         goto skip;
 248                 }
 249             }
 250             min_delta = delta;
 251             shift = i;
 252 skip:;
 253         }
 254
 255         /* overlap fading-out previous frame with fading-in current frame */
 256         curr = buf_in[0] + next_frame + shift;
 257         prev = buf_in[0] + prev_frame;
 258         d = dest[0];
 259         assert(next_frame + shift + st->dst_step - 1 < data_len);
 260         assert(prev_frame + st->dst_step - 1 < data_len);
 261         assert(dest[0] - buf_out[0] + st->dst_step - 1 < out_size);
 262         for (i = 0, j = st->dst_step; j; i++, j--)
 263         {
 264             *d++ = (*curr++ * (int64_t)i
 265                    + *prev++ * (int64_t)j) >> st->dst_order;
 266         }
 267         dest[0] = d;
 268         if (stereo)
 269         {
 270             curr = buf_in[1] +next_frame + shift;
 271             prev = buf_in[1] +prev_frame;
 272             d = dest[1];
 273             for (i = 0, j = st->dst_step; j; i++, j--)
 274             {
 275                 assert(d < buf_out[1] +out_size);
 276                 *d++ = (*curr++ * (int64_t) i
 277                         + *prev++ * (int64_t) j) >> st->dst_order;
 278             }
 279             dest[1] = d;
 280         }
 281
 282         /* adjust pointers for next frame */
 283         prev_frame = next_frame + shift + st->dst_step;
 284         next_frame += st->src_step;
 285
 286         /* here next_frame - prev_frame = src_step - dst_step - shift */
 287         assert(next_frame - prev_frame == st->src_step - st->dst_step - shift);
 288     }
 289
 290     /* now deal with remaining partial frames */
 291     if (last == -1)
 292     {
 293         /* special overlap buffer processing: remember frame shift only */
 294         st->ovl_shift = next_frame - prev_frame;
 295     }
 296     else if (last != 0)
 297     {
 298         /* last call: purge all remaining data to output buffer */
 299         i = data_len -prev_frame;
 300         assert(dest[0] +i <= buf_out[0] +out_size);
 301         memcpy(dest[0], buf_in[0] +prev_frame, i * sizeof(int32_t));
 302         dest[0] += i;
 303         if (stereo)
 304         {
 305             assert(dest[1] +i <= buf_out[1] +out_size);
 306             memcpy(dest[1], buf_in[1] +prev_frame, i * sizeof(int32_t));
 307             dest[1] += i;
 308         }
 309     }
 310     else
 311     {
 312         /* preserve remaining data + needed overlap data for next call */
 313         st->ovl_shift = next_frame - prev_frame;
 314         i = (st->ovl_shift < 0) ? next_frame : prev_frame;
 315         st->ovl_size = data_len - i;
 316         assert(st->ovl_size <= FIXED_BUFSIZE);
 317         memcpy(st->ovl_buff[0], buf_in[0]+i, st->ovl_size * sizeof(int32_t));
 318         if (stereo)
 319             memcpy(st->ovl_buff[1], buf_in[1]+i, st->ovl_size * sizeof(int32_t));
 320     }
 321
 322     return dest[0] - buf_out[0];
 323 }
 324
 325 long tdspeed_est_output_size()
 326 {
 327     return TDSPEED_OUTBUFSIZE;
 328 }
 329
 330 long tdspeed_est_input_size(long size)
 331 {
 332     struct tdspeed_state_s *st = &tdspeed_state;
 333     size = (size -st->ovl_size) *st->src_step / st->dst_step;
 334     if (size < 0)
 335         size = 0;
 336     return size;
 337 }
 338
 339 int tdspeed_doit(int32_t *src[], int count)
 340 {
 341     count = tdspeed_apply( (int32_t *[2]) { outbuf[0], outbuf[1] },
 342                            src, count, 0, TDSPEED_OUTBUFSIZE);
 343     src[0] = outbuf[0];
 344     src[1] = outbuf[1];
 345     return count;
 346 }
 347