]> code.delx.au - pulseaudio/blob - src/pulsecore/resampler.c
Fix channel remapping in resample; other modernizations
[pulseaudio] / src / pulsecore / resampler.c
1 /* $Id$ */
2
3 /***
4 This file is part of PulseAudio.
5
6 Copyright 2004-2006 Lennart Poettering
7
8 PulseAudio is free software; you can redistribute it and/or modify
9 it under the terms of the GNU Lesser General Public License as published
10 by the Free Software Foundation; either version 2 of the License,
11 or (at your option) any later version.
12
13 PulseAudio is distributed in the hope that it will be useful, but
14 WITHOUT ANY WARRANTY; without even the implied warranty of
15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 General Public License for more details.
17
18 You should have received a copy of the GNU Lesser General Public License
19 along with PulseAudio; if not, write to the Free Software
20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
21 USA.
22 ***/
23
24 #ifdef HAVE_CONFIG_H
25 #include <config.h>
26 #endif
27
28 #include <string.h>
29
30 #include <samplerate.h>
31 #include <liboil/liboilfuncs.h>
32 #include <liboil/liboil.h>
33
34 #include <pulse/xmalloc.h>
35 #include <pulsecore/sconv.h>
36 #include <pulsecore/log.h>
37 #include <pulsecore/macro.h>
38
39 #include "resampler.h"
40
41 struct pa_resampler {
42 pa_resample_method_t resample_method;
43 pa_sample_spec i_ss, o_ss;
44 pa_channel_map i_cm, o_cm;
45 size_t i_fz, o_fz;
46 pa_mempool *mempool;
47
48 void (*impl_free)(pa_resampler *r);
49 void (*impl_update_input_rate)(pa_resampler *r, uint32_t rate);
50 void (*impl_update_output_rate)(pa_resampler *r, uint32_t rate);
51 void (*impl_run)(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out);
52 void *impl_data;
53 };
54
55 struct impl_libsamplerate {
56 pa_memchunk buf1, buf2, buf3, buf4;
57 unsigned buf1_samples, buf2_samples, buf3_samples, buf4_samples;
58
59 pa_convert_to_float32ne_func_t to_float32ne_func;
60 pa_convert_from_float32ne_func_t from_float32ne_func;
61 SRC_STATE *src_state;
62
63 int map_table[PA_CHANNELS_MAX][PA_CHANNELS_MAX];
64 int map_required;
65 };
66
67 struct impl_trivial {
68 unsigned o_counter;
69 unsigned i_counter;
70 };
71
72 static int libsamplerate_init(pa_resampler*r);
73 static int trivial_init(pa_resampler*r);
74
75 pa_resampler* pa_resampler_new(
76 pa_mempool *pool,
77 const pa_sample_spec *a,
78 const pa_channel_map *am,
79 const pa_sample_spec *b,
80 const pa_channel_map *bm,
81 pa_resample_method_t resample_method) {
82
83 pa_resampler *r = NULL;
84
85 pa_assert(pool);
86 pa_assert(a);
87 pa_assert(b);
88 pa_assert(pa_sample_spec_valid(a));
89 pa_assert(pa_sample_spec_valid(b));
90 pa_assert(resample_method != PA_RESAMPLER_INVALID);
91
92 r = pa_xnew(pa_resampler, 1);
93 r->impl_data = NULL;
94 r->mempool = pool;
95 r->resample_method = resample_method;
96
97 r->impl_free = NULL;
98 r->impl_update_input_rate = NULL;
99 r->impl_run = NULL;
100
101 /* Fill sample specs */
102 r->i_ss = *a;
103 r->o_ss = *b;
104
105 if (am)
106 r->i_cm = *am;
107 else
108 pa_channel_map_init_auto(&r->i_cm, r->i_ss.channels, PA_CHANNEL_MAP_DEFAULT);
109
110 if (bm)
111 r->o_cm = *bm;
112 else
113 pa_channel_map_init_auto(&r->o_cm, r->o_ss.channels, PA_CHANNEL_MAP_DEFAULT);
114
115 r->i_fz = pa_frame_size(a);
116 r->o_fz = pa_frame_size(b);
117
118 /* Choose implementation */
119 if (a->channels != b->channels ||
120 a->format != b->format ||
121 !pa_channel_map_equal(&r->i_cm, &r->o_cm) ||
122 resample_method != PA_RESAMPLER_TRIVIAL) {
123
124 /* Use the libsamplerate based resampler for the complicated cases */
125 if (resample_method == PA_RESAMPLER_TRIVIAL)
126 r->resample_method = PA_RESAMPLER_SRC_ZERO_ORDER_HOLD;
127
128 if (libsamplerate_init(r) < 0)
129 goto fail;
130
131 } else {
132 /* Use our own simple non-fp resampler for the trivial cases and when the user selects it */
133 if (trivial_init(r) < 0)
134 goto fail;
135 }
136
137 return r;
138
139 fail:
140 if (r)
141 pa_xfree(r);
142
143 return NULL;
144 }
145
146 void pa_resampler_free(pa_resampler *r) {
147 pa_assert(r);
148
149 if (r->impl_free)
150 r->impl_free(r);
151
152 pa_xfree(r);
153 }
154
155 void pa_resampler_set_input_rate(pa_resampler *r, uint32_t rate) {
156 pa_assert(r);
157 pa_assert(rate > 0);
158
159 if (r->i_ss.rate == rate)
160 return;
161
162 r->i_ss.rate = rate;
163
164 if (r->impl_update_input_rate)
165 r->impl_update_input_rate(r, rate);
166 }
167
168 void pa_resampler_set_output_rate(pa_resampler *r, uint32_t rate) {
169 pa_assert(r);
170 pa_assert(rate > 0);
171
172 if (r->o_ss.rate == rate)
173 return;
174
175 r->o_ss.rate = rate;
176
177 if (r->impl_update_output_rate)
178 r->impl_update_output_rate(r, rate);
179 }
180
181 void pa_resampler_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) {
182 pa_assert(r && in && out && r->impl_run);
183
184 r->impl_run(r, in, out);
185 }
186
187 size_t pa_resampler_request(pa_resampler *r, size_t out_length) {
188 pa_assert(r);
189
190 return (((out_length / r->o_fz)*r->i_ss.rate)/r->o_ss.rate) * r->i_fz;
191 }
192
193 pa_resample_method_t pa_resampler_get_method(pa_resampler *r) {
194 pa_assert(r);
195 return r->resample_method;
196 }
197
198 static const char * const resample_methods[] = {
199 "src-sinc-best-quality",
200 "src-sinc-medium-quality",
201 "src-sinc-fastest",
202 "src-zero-order-hold",
203 "src-linear",
204 "trivial"
205 };
206
207 const char *pa_resample_method_to_string(pa_resample_method_t m) {
208
209 if (m < 0 || m >= PA_RESAMPLER_MAX)
210 return NULL;
211
212 return resample_methods[m];
213 }
214
215 pa_resample_method_t pa_parse_resample_method(const char *string) {
216 pa_resample_method_t m;
217
218 pa_assert(string);
219
220 for (m = 0; m < PA_RESAMPLER_MAX; m++)
221 if (!strcmp(string, resample_methods[m]))
222 return m;
223
224 return PA_RESAMPLER_INVALID;
225 }
226
227
228 /*** libsamplerate based implementation ***/
229
230 static void libsamplerate_free(pa_resampler *r) {
231 struct impl_libsamplerate *u;
232
233 pa_assert(r);
234 pa_assert(r->impl_data);
235
236 u = r->impl_data;
237
238 if (u->src_state)
239 src_delete(u->src_state);
240
241 if (u->buf1.memblock)
242 pa_memblock_unref(u->buf1.memblock);
243 if (u->buf2.memblock)
244 pa_memblock_unref(u->buf2.memblock);
245 if (u->buf3.memblock)
246 pa_memblock_unref(u->buf3.memblock);
247 if (u->buf4.memblock)
248 pa_memblock_unref(u->buf4.memblock);
249 pa_xfree(u);
250 }
251
252 static void calc_map_table(pa_resampler *r) {
253 struct impl_libsamplerate *u;
254 unsigned oc;
255
256 pa_assert(r);
257 pa_assert(r->impl_data);
258
259 u = r->impl_data;
260
261 if (!(u->map_required = (r->i_ss.channels != r->o_ss.channels || !pa_channel_map_equal(&r->i_cm, &r->o_cm))))
262 return;
263
264 for (oc = 0; oc < r->o_ss.channels; oc++) {
265 unsigned ic, i = 0;
266
267 for (ic = 0; ic < r->i_ss.channels; ic++) {
268 pa_channel_position_t a, b;
269
270 a = r->i_cm.map[ic];
271 b = r->o_cm.map[oc];
272
273 if (a == b ||
274 (a == PA_CHANNEL_POSITION_MONO && b == PA_CHANNEL_POSITION_LEFT) ||
275 (a == PA_CHANNEL_POSITION_MONO && b == PA_CHANNEL_POSITION_RIGHT) ||
276 (a == PA_CHANNEL_POSITION_LEFT && b == PA_CHANNEL_POSITION_MONO) ||
277 (a == PA_CHANNEL_POSITION_RIGHT && b == PA_CHANNEL_POSITION_MONO))
278
279 u->map_table[oc][i++] = ic;
280 }
281
282 /* Add an end marker */
283 if (i < PA_CHANNELS_MAX)
284 u->map_table[oc][i] = -1;
285 }
286 }
287
288 static pa_memchunk* convert_to_float(pa_resampler *r, pa_memchunk *input) {
289 struct impl_libsamplerate *u;
290 unsigned n_samples;
291 void *src, *dst;
292
293 pa_assert(r);
294 pa_assert(input);
295 pa_assert(input->memblock);
296
297 pa_assert(r->impl_data);
298 u = r->impl_data;
299
300 /* Convert the incoming sample into floats and place them in buf1 */
301
302 if (!u->to_float32ne_func || !input->length)
303 return input;
304
305 n_samples = (input->length / r->i_fz) * r->i_ss.channels;
306
307 if (!u->buf1.memblock || u->buf1_samples < n_samples) {
308 if (u->buf1.memblock)
309 pa_memblock_unref(u->buf1.memblock);
310
311 u->buf1_samples = n_samples;
312 u->buf1.memblock = pa_memblock_new(r->mempool, u->buf1.length = sizeof(float) * n_samples);
313 u->buf1.index = 0;
314 }
315
316 src = (uint8_t*) pa_memblock_acquire(input->memblock) + input->index;
317 dst = (uint8_t*) pa_memblock_acquire(u->buf1.memblock);
318
319 u->to_float32ne_func(n_samples, src, dst);
320
321 pa_memblock_release(input->memblock);
322 pa_memblock_release(u->buf1.memblock);
323
324 u->buf1.length = sizeof(float) * n_samples;
325
326 return &u->buf1;
327 }
328
329 static pa_memchunk *remap_channels(pa_resampler *r, pa_memchunk *input) {
330 struct impl_libsamplerate *u;
331 unsigned in_n_samples, out_n_samples, n_frames;
332 int i_skip, o_skip;
333 unsigned oc;
334 float *src, *dst;
335
336 pa_assert(r);
337 pa_assert(input);
338 pa_assert(input->memblock);
339
340 pa_assert(r->impl_data);
341 u = r->impl_data;
342
343 /* Remap channels and place the result int buf2 */
344
345 if (!u->map_required || !input->length)
346 return input;
347
348 in_n_samples = input->length / sizeof(float);
349 n_frames = in_n_samples / r->i_ss.channels;
350 out_n_samples = n_frames * r->o_ss.channels;
351
352 if (!u->buf2.memblock || u->buf2_samples < out_n_samples) {
353 if (u->buf2.memblock)
354 pa_memblock_unref(u->buf2.memblock);
355
356 u->buf2_samples = out_n_samples;
357 u->buf2.memblock = pa_memblock_new(r->mempool, u->buf2.length = sizeof(float) * out_n_samples);
358 u->buf2.index = 0;
359 }
360
361 src = (float*) ((uint8_t*) pa_memblock_acquire(input->memblock) + input->index);
362 dst = (float*) pa_memblock_acquire(u->buf2.memblock);
363
364 memset(dst, 0, u->buf2.length);
365
366 o_skip = sizeof(float) * r->o_ss.channels;
367 i_skip = sizeof(float) * r->i_ss.channels;
368
369 for (oc = 0; oc < r->o_ss.channels; oc++) {
370 unsigned i;
371 static const float one = 1.0;
372
373 for (i = 0; i < PA_CHANNELS_MAX && u->map_table[oc][i] >= 0; i++)
374 oil_vectoradd_f32(
375 dst + oc, o_skip,
376 dst + oc, o_skip,
377 src + u->map_table[oc][i], i_skip,
378 n_frames,
379 &one, &one);
380 }
381
382 pa_memblock_release(input->memblock);
383 pa_memblock_release(u->buf2.memblock);
384
385 u->buf2.length = out_n_samples * sizeof(float);
386
387 return &u->buf2;
388 }
389
390 static pa_memchunk *resample(pa_resampler *r, pa_memchunk *input) {
391 struct impl_libsamplerate *u;
392 SRC_DATA data;
393 unsigned in_n_frames, in_n_samples;
394 unsigned out_n_frames, out_n_samples;
395 int ret;
396
397 pa_assert(r);
398 pa_assert(input);
399 pa_assert(r->impl_data);
400 u = r->impl_data;
401
402 /* Resample the data and place the result in buf3 */
403
404 if (!u->src_state || !input->length)
405 return input;
406
407 in_n_samples = input->length / sizeof(float);
408 in_n_frames = in_n_samples / r->o_ss.channels;
409
410 out_n_frames = ((in_n_frames*r->o_ss.rate)/r->i_ss.rate)+1024;
411 out_n_samples = out_n_frames * r->o_ss.channels;
412
413 if (!u->buf3.memblock || u->buf3_samples < out_n_samples) {
414 if (u->buf3.memblock)
415 pa_memblock_unref(u->buf3.memblock);
416
417 u->buf3_samples = out_n_samples;
418 u->buf3.memblock = pa_memblock_new(r->mempool, u->buf3.length = sizeof(float) * out_n_samples);
419 u->buf3.index = 0;
420 }
421
422 data.data_in = (float*) ((uint8_t*) pa_memblock_acquire(input->memblock) + input->index);
423 data.input_frames = in_n_frames;
424
425 data.data_out = (float*) pa_memblock_acquire(u->buf3.memblock);
426 data.output_frames = out_n_frames;
427
428 data.src_ratio = (double) r->o_ss.rate / r->i_ss.rate;
429 data.end_of_input = 0;
430
431 ret = src_process(u->src_state, &data);
432 pa_assert(ret == 0);
433 pa_assert((unsigned) data.input_frames_used == in_n_frames);
434
435 pa_memblock_release(input->memblock);
436 pa_memblock_release(u->buf3.memblock);
437
438 u->buf3.length = data.output_frames_gen * sizeof(float) * r->o_ss.channels;
439
440 return &u->buf3;
441 }
442
443 static pa_memchunk *convert_from_float(pa_resampler *r, pa_memchunk *input) {
444 struct impl_libsamplerate *u;
445 unsigned n_samples, n_frames;
446 void *src, *dst;
447
448 pa_assert(r);
449 pa_assert(input);
450 pa_assert(r->impl_data);
451 u = r->impl_data;
452
453 /* Convert the data into the correct sample type and place the result in buf4 */
454
455 if (!u->from_float32ne_func || !input->length)
456 return input;
457
458 n_frames = input->length / sizeof(float) / r->o_ss.channels;
459 n_samples = n_frames * r->o_ss.channels;
460
461 if (!u->buf4.memblock || u->buf4_samples < n_samples) {
462 if (u->buf4.memblock)
463 pa_memblock_unref(u->buf4.memblock);
464
465 u->buf4_samples = n_samples;
466 u->buf4.memblock = pa_memblock_new(r->mempool, u->buf4.length = r->o_fz * n_frames);
467 u->buf4.index = 0;
468 }
469
470 src = (uint8_t*) pa_memblock_acquire(input->memblock) + input->index;
471 dst = pa_memblock_acquire(u->buf4.memblock);
472 u->from_float32ne_func(n_samples, src, dst);
473 pa_memblock_release(input->memblock);
474 pa_memblock_release(u->buf4.memblock);
475
476 u->buf4.length = r->o_fz * n_frames;
477
478 return &u->buf4;
479 }
480
481 static void libsamplerate_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) {
482 struct impl_libsamplerate *u;
483 pa_memchunk *buf;
484
485 pa_assert(r);
486 pa_assert(in);
487 pa_assert(out);
488 pa_assert(in->length);
489 pa_assert(in->memblock);
490 pa_assert(in->length % r->i_fz == 0);
491 pa_assert(r->impl_data);
492
493 u = r->impl_data;
494
495 buf = (pa_memchunk*) in;
496 buf = convert_to_float(r, buf);
497 buf = remap_channels(r, buf);
498 buf = resample(r, buf);
499
500 if (buf->length) {
501 buf = convert_from_float(r, buf);
502 *out = *buf;
503
504 if (buf == in)
505 pa_memblock_ref(buf->memblock);
506 else
507 pa_memchunk_reset(buf);
508 } else
509 pa_memchunk_reset(out);
510 }
511
512 static void libsamplerate_update_input_rate(pa_resampler *r, uint32_t rate) {
513 struct impl_libsamplerate *u;
514
515 pa_assert(r);
516 pa_assert(rate > 0);
517 pa_assert(r->impl_data);
518 u = r->impl_data;
519
520 if (!u->src_state) {
521 int err;
522 u->src_state = src_new(r->resample_method, r->o_ss.channels, &err);
523 pa_assert(u->src_state);
524 } else {
525 int ret = src_set_ratio(u->src_state, (double) r->o_ss.rate / rate);
526 pa_assert(ret == 0);
527 }
528 }
529
530 static void libsamplerate_update_output_rate(pa_resampler *r, uint32_t rate) {
531 struct impl_libsamplerate *u;
532
533 pa_assert(r);
534 pa_assert(rate > 0);
535 pa_assert(r->impl_data);
536 u = r->impl_data;
537
538 if (!u->src_state) {
539 int err;
540 u->src_state = src_new(r->resample_method, r->o_ss.channels, &err);
541 pa_assert(u->src_state);
542 } else {
543 int ret = src_set_ratio(u->src_state, (double) rate / r->i_ss.rate);
544 pa_assert(ret == 0);
545 }
546 }
547
548 static int libsamplerate_init(pa_resampler *r) {
549 struct impl_libsamplerate *u = NULL;
550 int err;
551
552 r->impl_data = u = pa_xnew(struct impl_libsamplerate, 1);
553
554 pa_memchunk_reset(&u->buf1);
555 pa_memchunk_reset(&u->buf2);
556 pa_memchunk_reset(&u->buf3);
557 pa_memchunk_reset(&u->buf4);
558 u->buf1_samples = u->buf2_samples = u->buf3_samples = u->buf4_samples = 0;
559
560 if (r->i_ss.format == PA_SAMPLE_FLOAT32NE)
561 u->to_float32ne_func = NULL;
562 else if (!(u->to_float32ne_func = pa_get_convert_to_float32ne_function(r->i_ss.format)))
563 goto fail;
564
565 if (r->o_ss.format == PA_SAMPLE_FLOAT32NE)
566 u->from_float32ne_func = NULL;
567 else if (!(u->from_float32ne_func = pa_get_convert_from_float32ne_function(r->o_ss.format)))
568 goto fail;
569
570 if (r->o_ss.rate == r->i_ss.rate)
571 u->src_state = NULL;
572 else if (!(u->src_state = src_new(r->resample_method, r->o_ss.channels, &err)))
573 goto fail;
574
575 r->impl_free = libsamplerate_free;
576 r->impl_update_input_rate = libsamplerate_update_input_rate;
577 r->impl_update_output_rate = libsamplerate_update_output_rate;
578 r->impl_run = libsamplerate_run;
579
580 calc_map_table(r);
581
582 return 0;
583
584 fail:
585 pa_xfree(u);
586 return -1;
587 }
588
589 /* Trivial implementation */
590
591 static void trivial_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) {
592 size_t fz;
593 unsigned n_frames;
594 struct impl_trivial *u;
595
596 pa_assert(r);
597 pa_assert(in);
598 pa_assert(out);
599 pa_assert(r->impl_data);
600
601 u = r->impl_data;
602
603 fz = r->i_fz;
604 pa_assert(fz == r->o_fz);
605
606 n_frames = in->length/fz;
607
608 if (r->i_ss.rate == r->o_ss.rate) {
609
610 /* In case there's no diefference in sample types, do nothing */
611 *out = *in;
612 pa_memblock_ref(out->memblock);
613
614 u->o_counter += n_frames;
615 } else {
616 /* Do real resampling */
617 size_t l;
618 unsigned o_index;
619 void *src, *dst;
620
621 /* The length of the new memory block rounded up */
622 l = ((((n_frames+1) * r->o_ss.rate) / r->i_ss.rate) + 1) * fz;
623
624 out->index = 0;
625 out->memblock = pa_memblock_new(r->mempool, l);
626
627 src = (uint8_t*) pa_memblock_acquire(in->memblock) + in->index;
628 dst = pa_memblock_acquire(out->memblock);
629
630 for (o_index = 0;; o_index++, u->o_counter++) {
631 unsigned j;
632
633 j = (u->o_counter * r->i_ss.rate / r->o_ss.rate);
634 j = j > u->i_counter ? j - u->i_counter : 0;
635
636 if (j >= n_frames)
637 break;
638
639 pa_assert(o_index*fz < pa_memblock_get_length(out->memblock));
640
641 memcpy((uint8_t*) dst + fz*o_index,
642 (uint8_t*) src + fz*j, fz);
643
644 }
645
646 pa_memblock_release(in->memblock);
647 pa_memblock_release(out->memblock);
648
649 out->length = o_index*fz;
650 }
651
652 u->i_counter += n_frames;
653
654 /* Normalize counters */
655 while (u->i_counter >= r->i_ss.rate) {
656 u->i_counter -= r->i_ss.rate;
657 pa_assert(u->o_counter >= r->o_ss.rate);
658 u->o_counter -= r->o_ss.rate;
659 }
660 }
661
662 static void trivial_free(pa_resampler *r) {
663 pa_assert(r);
664
665 pa_xfree(r->impl_data);
666 }
667
668 static void trivial_update_rate(pa_resampler *r, uint32_t rate) {
669 struct impl_trivial *u;
670
671 pa_assert(r);
672 pa_assert(rate > 0);
673 pa_assert(r->impl_data);
674
675 u = r->impl_data;
676 u->i_counter = 0;
677 u->o_counter = 0;
678 }
679
680 static int trivial_init(pa_resampler*r) {
681 struct impl_trivial *u;
682
683 pa_assert(r);
684 pa_assert(r->i_ss.format == r->o_ss.format);
685 pa_assert(r->i_ss.channels == r->o_ss.channels);
686
687 r->impl_data = u = pa_xnew(struct impl_trivial, 1);
688 u->o_counter = u->i_counter = 0;
689
690 r->impl_run = trivial_run;
691 r->impl_free = trivial_free;
692 r->impl_update_input_rate = trivial_update_rate;
693 r->impl_update_output_rate = trivial_update_rate;
694
695 return 0;
696 }
697
698