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