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