diff options
Diffstat (limited to 'lib/rbcodec/dsp/tdspeed.c')
-rw-r--r-- | lib/rbcodec/dsp/tdspeed.c | 450 |
1 files changed, 450 insertions, 0 deletions
diff --git a/lib/rbcodec/dsp/tdspeed.c b/lib/rbcodec/dsp/tdspeed.c new file mode 100644 index 0000000000..731be12621 --- /dev/null +++ b/lib/rbcodec/dsp/tdspeed.c | |||
@@ -0,0 +1,450 @@ | |||
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 *dest[2]; | ||
206 | int32_t next_frame, prev_frame, 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 | int 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 | int const INC1 = 8; | ||
291 | int const INC2 = 32; | ||
292 | |||
293 | int64_t min_delta = INT64_MAX; /* most positive */ | ||
294 | int 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 (int i = 0; i < st->shift_max; i += INC1) | ||
303 | { | ||
304 | int64_t delta = 0; | ||
305 | |||
306 | int32_t *curr = buf_in[0] + next_frame + i; | ||
307 | int32_t *prev = buf_in[0] + prev_frame; | ||
308 | |||
309 | for (int j = 0; j < st->dst_step; j += INC2, curr += INC2, prev += INC2) | ||
310 | { | ||
311 | int32_t diff = *curr - *prev; | ||
312 | delta += abs(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 (int j = 0; j < st->dst_step; j += INC2, curr += INC2, prev += INC2) | ||
324 | { | ||
325 | int32_t diff = *curr - *prev; | ||
326 | delta += abs(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 | int32_t *curr = buf_in[0] + next_frame + shift; | ||
340 | int32_t *prev = buf_in[0] + prev_frame; | ||
341 | |||
342 | int32_t *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 (int 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 (int 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 | int 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 | int 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 | |||