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** dsp_src;
  42 static int handles[4];
  43 static int32_t *overlap_buffer[2] = { NULL, NULL };
  44 static int32_t *outbuf[2] = { NULL, NULL };
  45
  46 static int move_callback(int handle, void* current, void* new)
  47 {
  48     /* TODO */
  49     (void)handle;
  50     if (dsp_src)
  51     {
  52         int ch = (current == outbuf[0]) ? 0 : 1;
  53         dsp_src[ch] = outbuf[ch] = new;
  54     }
  55     return BUFLIB_CB_OK;
  56 }
  57
  58 static struct buflib_callbacks ops = {
  59     .move_callback = move_callback,
  60     .shrink_callback = NULL,
  61 };
  62
  63 static int ovl_move_callback(int handle, void* current, void* new)
  64 {
  65     /* TODO */
  66     (void)handle;
  67     if (dsp_src)
  68     {
  69         int ch = (current == overlap_buffer[0]) ? 0 : 1;
  70         overlap_buffer[ch] = new;
  71     }
  72     return BUFLIB_CB_OK;
  73 }
  74
  75 static struct buflib_callbacks ovl_ops = {
  76     .move_callback = ovl_move_callback,
  77     .shrink_callback = NULL,
  78 };
  79
  80
  81 static struct tdspeed_state_s
  82 {
  83     bool stereo;
  84     int32_t shift_max;      /* maximum displacement on a frame */
  85     int32_t src_step;       /* source window pace */
  86     int32_t dst_step;       /* destination window pace */
  87     int32_t dst_order;      /* power of two for dst_step */
  88     int32_t ovl_shift;      /* overlap buffer frame shift */
  89     int32_t ovl_size;       /* overlap buffer used size */
  90     int32_t ovl_space;      /* overlap buffer size */
  91     int32_t *ovl_buff[2];   /* overlap buffer */
  92 } tdspeed_state;
  93
  94 void tdspeed_init(void)
  95 {
  96     if (!global_settings.timestretch_enabled)
  97         return;
  98
  99     /* Allocate buffers */
 100     if (overlap_buffer[0] == NULL)
 101     {
 102         handles[0] = core_alloc_ex("tdspeed ovl left", FIXED_BUFSIZE * sizeof(int32_t), &ovl_ops);
 103         overlap_buffer[0] = core_get_data(handles[0]);
 104     }
 105     if (overlap_buffer[1] == NULL)
 106     {
 107         handles[1] = core_alloc_ex("tdspeed ovl right", FIXED_BUFSIZE * sizeof(int32_t), &ovl_ops);
 108         overlap_buffer[1] = core_get_data(handles[1]);
 109     }
 110     if (outbuf[0] == NULL)
 111     {
 112         handles[2] = core_alloc_ex("tdspeed left", TDSPEED_OUTBUFSIZE * sizeof(int32_t), &ops);
 113         outbuf[0] = core_get_data(handles[2]);
 114     }
 115     if (outbuf[1] == NULL)
 116     {
 117         handles[3] = core_alloc_ex("tdspeed right", TDSPEED_OUTBUFSIZE * sizeof(int32_t), &ops);
 118         outbuf[1] = core_get_data(handles[3]);
 119     }
 120 }
 121
 122 void tdspeed_finish(void)
 123 {
 124     for(unsigned i = 0; i < ARRAYLEN(handles); i++)
 125     {
 126         if (handles[i] > 0)
 127         {
 128             core_free(handles[i]);
 129             handles[i] = 0;
 130         }
 131     }
 132     overlap_buffer[0] = overlap_buffer[1] = NULL;
 133     outbuf[0]         = outbuf[1]         = NULL;
 134 }
 135
 136 bool tdspeed_config(int samplerate, bool stereo, int32_t factor)
 137 {
 138     struct tdspeed_state_s *st = &tdspeed_state;
 139     int src_frame_sz;
 140
 141     /* Check buffers were allocated ok */
 142     if (overlap_buffer[0] == NULL || overlap_buffer[1] == NULL)
 143         return false;
 144
 145     if (outbuf[0] == NULL || outbuf[1] == NULL)
 146         return false;
 147
 148     /* Check parameters */
 149     if (factor == PITCH_SPEED_100)
 150         return false;
 151
 152     if (samplerate < MIN_RATE || samplerate > MAX_RATE)
 153         return false;
 154
 155     if (factor < STRETCH_MIN || factor > STRETCH_MAX)
 156         return false;
 157
 158     st->stereo = stereo;
 159     st->dst_step = samplerate / MINFREQ;
 160
 161     if (factor > PITCH_SPEED_100)
 162         st->dst_step = st->dst_step * PITCH_SPEED_100 / factor;
 163
 164     st->dst_order = 1;
 165
 166     while (st->dst_step >>= 1)
 167         st->dst_order++;
 168
 169     st->dst_step = (1 << st->dst_order);
 170     st->src_step = st->dst_step * factor / PITCH_SPEED_100;
 171     st->shift_max = (st->dst_step > st->src_step) ? st->dst_step : st->src_step;
 172
 173     src_frame_sz = st->shift_max + st->dst_step;
 174
 175     if (st->dst_step > st->src_step)
 176         src_frame_sz += st->dst_step - st->src_step;
 177
 178     st->ovl_space = ((src_frame_sz - 2) / st->src_step) * st->src_step
 179                         + src_frame_sz;
 180
 181     if (st->src_step > st->dst_step)
 182         st->ovl_space += 2*st->src_step - st->dst_step;
 183
 184     if (st->ovl_space > FIXED_BUFSIZE)
 185         st->ovl_space = FIXED_BUFSIZE;
 186
 187     st->ovl_size = 0;
 188     st->ovl_shift = 0;
 189
 190     st->ovl_buff[0] = overlap_buffer[0];
 191
 192     if (stereo)
 193         st->ovl_buff[1] = overlap_buffer[1];
 194     else
 195         st->ovl_buff[1] = st->ovl_buff[0];
 196
 197     return true;
 198 }
 199
 200 static int tdspeed_apply(int32_t *buf_out[2], int32_t *buf_in[2],
 201                          int data_len, int last, int out_size)
 202 /* data_len in samples */
 203 {
 204     struct tdspeed_state_s *st = &tdspeed_state;
 205     int32_t *curr, *prev, *dest[2], *d;
 206     int32_t i, j, next_frame, prev_frame, shift, src_frame_sz;
 207     bool stereo = buf_in[0] != buf_in[1];
 208
 209     assert(stereo == st->stereo);
 210
 211     src_frame_sz = st->shift_max + st->dst_step;
 212
 213     if (st->dst_step > st->src_step)
 214         src_frame_sz += st->dst_step - st->src_step;
 215
 216     /* deal with overlap data first, if any */
 217     if (st->ovl_size)
 218     {
 219         int32_t have, copy, steps;
 220         have = st->ovl_size;
 221
 222         if (st->ovl_shift > 0)
 223             have -= st->ovl_shift;
 224
 225         /* append just enough data to have all of the overlap buffer consumed */
 226         steps = (have - 1) / st->src_step;
 227         copy = steps * st->src_step + src_frame_sz - have;
 228
 229         if (copy < src_frame_sz - st->dst_step)
 230             copy += st->src_step;  /* one more step to allow for pregap data */
 231
 232         if (copy > data_len)
 233             copy = data_len;
 234
 235         assert(st->ovl_size + copy <= FIXED_BUFSIZE);
 236         memcpy(st->ovl_buff[0] + st->ovl_size, buf_in[0],
 237                copy * sizeof(int32_t));
 238
 239         if (stereo)
 240             memcpy(st->ovl_buff[1] + st->ovl_size, buf_in[1],
 241                    copy * sizeof(int32_t));
 242
 243         if (!last && have + copy < src_frame_sz)
 244         {
 245             /* still not enough to process at least one frame */
 246             st->ovl_size += copy;
 247             return 0;
 248         }
 249
 250         /* recursively call ourselves to process the overlap buffer */
 251         have = st->ovl_size;
 252         st->ovl_size = 0;
 253
 254         if (copy == data_len)
 255         {
 256             assert(have + copy <= FIXED_BUFSIZE);
 257             return tdspeed_apply(buf_out, st->ovl_buff, have+copy, last,
 258                                out_size);
 259         }
 260
 261         assert(have + copy <= FIXED_BUFSIZE);
 262         i = tdspeed_apply(buf_out, st->ovl_buff, have+copy, -1, out_size);
 263
 264         dest[0] = buf_out[0] + i;
 265         dest[1] = buf_out[1] + i;
 266
 267         /* readjust pointers to account for data already consumed */
 268         next_frame = copy - src_frame_sz + st->src_step;
 269         prev_frame = next_frame - st->ovl_shift;
 270     }
 271     else
 272     {
 273         dest[0] = buf_out[0];
 274         dest[1] = buf_out[1];
 275
 276         next_frame = prev_frame = 0;
 277
 278         if (st->ovl_shift > 0)
 279             next_frame += st->ovl_shift;
 280         else
 281             prev_frame += -st->ovl_shift;
 282     }
 283
 284     st->ovl_shift = 0;
 285
 286     /* process all complete frames */
 287     while (data_len - next_frame >= src_frame_sz)
 288     {
 289         /* find frame overlap by autocorelation */
 290         int32_t const INC1 = 8;
 291         int32_t const INC2 = 32;
 292
 293         int64_t min_delta = ~(1ll << 63);  /* most positive */
 294         shift = 0;
 295
 296         /* Power of 2 of a 28bit number requires 56bits, can accumulate
 297            256times in a 64bit variable. */
 298         assert(st->dst_step / INC2 <= 256);
 299         assert(next_frame + st->shift_max - 1 + st->dst_step - 1 < data_len);
 300         assert(prev_frame + st->dst_step - 1 < data_len);
 301
 302         for (i = 0; i < st->shift_max; i += INC1)
 303         {
 304             int64_t delta = 0;
 305
 306             curr = buf_in[0] + next_frame + i;
 307             prev = buf_in[0] + prev_frame;
 308
 309             for (j = 0; j < st->dst_step; j += INC2, curr += INC2, prev += INC2)
 310             {
 311                 int32_t diff = *curr - *prev;
 312                 delta += (int64_t)diff * diff;
 313
 314                 if (delta >= min_delta)
 315                     goto skip;
 316             }
 317
 318             if (stereo)
 319             {
 320                 curr = buf_in[1] + next_frame + i;
 321                 prev = buf_in[1] + prev_frame;
 322
 323                 for (j = 0; j < st->dst_step; j += INC2, curr += INC2, prev += INC2)
 324                 {
 325                     int32_t diff = *curr - *prev;
 326                     delta += (int64_t)diff * diff;
 327
 328                     if (delta >= min_delta)
 329                         goto skip;
 330                 }
 331             }
 332
 333             min_delta = delta;
 334             shift = i;
 335 skip:;
 336         }
 337
 338         /* overlap fading-out previous frame with fading-in current frame */
 339         curr = buf_in[0] + next_frame + shift;
 340         prev = buf_in[0] + prev_frame;
 341
 342         d = dest[0];
 343
 344         assert(next_frame + shift + st->dst_step - 1 < data_len);
 345         assert(prev_frame + st->dst_step - 1 < data_len);
 346         assert(dest[0] - buf_out[0] + st->dst_step - 1 < out_size);
 347
 348         for (i = 0, j = st->dst_step; j; i++, j--)
 349         {
 350             *d++ = (*curr++ * (int64_t)i +
 351                     *prev++ * (int64_t)j) >> st->dst_order;
 352         }
 353
 354         dest[0] = d;
 355
 356         if (stereo)
 357         {
 358             curr = buf_in[1] + next_frame + shift;
 359             prev = buf_in[1] + prev_frame;
 360
 361             d = dest[1];
 362
 363             for (i = 0, j = st->dst_step; j; i++, j--)
 364             {
 365                 assert(d < buf_out[1] + out_size);
 366
 367                 *d++ = (*curr++ * (int64_t)i +
 368                         *prev++ * (int64_t)j) >> st->dst_order;
 369             }
 370
 371             dest[1] = d;
 372         }
 373
 374         /* adjust pointers for next frame */
 375         prev_frame = next_frame + shift + st->dst_step;
 376         next_frame += st->src_step;
 377
 378         /* here next_frame - prev_frame = src_step - dst_step - shift */
 379         assert(next_frame - prev_frame == st->src_step - st->dst_step - shift);
 380     }
 381
 382     /* now deal with remaining partial frames */
 383     if (last == -1)
 384     {
 385         /* special overlap buffer processing: remember frame shift only */
 386         st->ovl_shift = next_frame - prev_frame;
 387     }
 388     else if (last != 0)
 389     {
 390         /* last call: purge all remaining data to output buffer */
 391         i = data_len - prev_frame;
 392
 393         assert(dest[0] + i <= buf_out[0] + out_size);
 394         memcpy(dest[0], buf_in[0] + prev_frame, i * sizeof(int32_t));
 395
 396         dest[0] += i;
 397
 398         if (stereo)
 399         {
 400             assert(dest[1] + i <= buf_out[1] + out_size);
 401             memcpy(dest[1], buf_in[1] + prev_frame, i * sizeof(int32_t));
 402             dest[1] += i;
 403         }
 404     }
 405     else
 406     {
 407         /* preserve remaining data + needed overlap data for next call */
 408         st->ovl_shift = next_frame - prev_frame;
 409         i = (st->ovl_shift < 0) ? next_frame : prev_frame;
 410         st->ovl_size = data_len - i;
 411
 412         assert(st->ovl_size <= FIXED_BUFSIZE);
 413         memcpy(st->ovl_buff[0], buf_in[0] + i, st->ovl_size * sizeof(int32_t));
 414
 415         if (stereo)
 416             memcpy(st->ovl_buff[1], buf_in[1] + i, st->ovl_size * sizeof(int32_t));
 417     }
 418
 419     return dest[0] - buf_out[0];
 420 }
 421
 422 long tdspeed_est_output_size()
 423 {
 424     return TDSPEED_OUTBUFSIZE;
 425 }
 426
 427 long tdspeed_est_input_size(long size)
 428 {
 429     struct tdspeed_state_s *st = &tdspeed_state;
 430
 431     size = (size - st->ovl_size) * st->src_step / st->dst_step;
 432
 433     if (size < 0)
 434         size = 0;
 435
 436     return size;
 437 }
 438
 439 int tdspeed_doit(int32_t *src[], int count)
 440 {
 441     dsp_src = src;
 442     count = tdspeed_apply( (int32_t *[2]) { outbuf[0], outbuf[1] },
 443                            src, count, 0, TDSPEED_OUTBUFSIZE);
 444
 445     src[0] = outbuf[0];
 446     src[1] = outbuf[1];
 447
 448     return count;
 449 }
 450