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