Extract config file saving code to a function; Do not write the NUL character to...
[kugel-rb.git] / apps / tdspeed.c
blob8cb495c08f8b9f8fe886598f945654ad14449630
1 /***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
10 * Copyright (C) 2006 by Nicolas Pitre <nico@cam.org>
11 * Copyright (C) 2006-2007 by Stéphane Doyon <s.doyon@videotron.ca>
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.
18 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
19 * KIND, either express or implied.
21 ****************************************************************************/
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"
33 #define assert(cond)
35 #define MIN_RATE 8000
36 #define MAX_RATE 48000 /* double buffer for double rate */
37 #define MINFREQ 100
39 #define FIXED_BUFSIZE 3072 /* 48KHz factor 3.0 */
41 struct tdspeed_state_s
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 */
53 static struct tdspeed_state_s tdspeed_state;
55 static int32_t *overlap_buffer[2] = { NULL, NULL };
56 static int32_t *outbuf[2] = { NULL, NULL };
58 void tdspeed_init()
60 if (global_settings.timestretch_enabled)
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));
75 bool tdspeed_config(int samplerate, bool stereo, int32_t factor)
77 struct tdspeed_state_s *st = &tdspeed_state;
78 int src_frame_sz;
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;
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;
94 st->stereo = stereo;
95 st->dst_step = samplerate / MINFREQ;
97 if (factor > PITCH_SPEED_100)
98 st->dst_step = st->dst_step * PITCH_SPEED_100 / factor;
99 st->dst_order = 1;
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;
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;
115 if (st->ovl_space > FIXED_BUFSIZE)
116 st->ovl_space = FIXED_BUFSIZE;
118 st->ovl_size = 0;
119 st->ovl_shift = 0;
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];
127 return true;
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 */
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);
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;
144 /* deal with overlap data first, if any */
145 if (st->ovl_size)
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)
165 /* still not enough to process at least one frame */
166 st->ovl_size += copy;
167 return 0;
170 /* recursively call ourselves to process the overlap buffer */
171 have = st->ovl_size;
172 st->ovl_size = 0;
173 if (copy == data_len)
175 assert( (have+copy) <= FIXED_BUFSIZE);
176 return tdspeed_apply(buf_out, st->ovl_buff, have+copy, last,
177 out_size);
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;
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;
188 else
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;
198 st->ovl_shift = 0;
200 /* process all complete frames */
201 while (data_len - next_frame >= src_frame_sz)
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)
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)
220 int32_t diff = *curr - *prev;
221 delta += (int64_t)diff * diff;
222 if (delta >= min_delta)
223 goto skip;
225 if (stereo)
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)
231 int32_t diff = *curr - *prev;
232 delta += (int64_t)diff * diff;
233 if (delta >= min_delta)
234 goto skip;
237 min_delta = delta;
238 shift = i;
239 skip:;
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--)
251 *d++ = (*curr++ * (int64_t)i
252 + *prev++ * (int64_t)j) >> st->dst_order;
254 dest[0] = d;
255 if (stereo)
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--)
262 assert(d < buf_out[1] +out_size);
263 *d++ = (*curr++ * (int64_t) i
264 + *prev++ * (int64_t) j) >> st->dst_order;
266 dest[1] = d;
269 /* adjust pointers for next frame */
270 prev_frame = next_frame + shift + st->dst_step;
271 next_frame += st->src_step;
273 /* here next_frame - prev_frame = src_step - dst_step - shift */
274 assert(next_frame - prev_frame == st->src_step - st->dst_step - shift);
277 /* now deal with remaining partial frames */
278 if (last == -1)
280 /* special overlap buffer processing: remember frame shift only */
281 st->ovl_shift = next_frame - prev_frame;
283 else if (last != 0)
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)
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;
297 else
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));
309 return dest[0] - buf_out[0];
312 long tdspeed_est_output_size()
314 return TDSPEED_OUTBUFSIZE;
317 long tdspeed_est_input_size(long size)
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;
326 int tdspeed_doit(int32_t *src[], int count)
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;