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 <assert.h>
  29 #include <string.h>
  30
  31 #include <samplerate.h>
  32 #include <liboil/liboilfuncs.h>
  33 #include <liboil/liboil.h>
  34
  35 #include <pulse/xmalloc.h>
  36
  37 #include <pulsecore/sconv.h>
  38 #include <pulsecore/log.h>
  39
  40 #include "resampler.h"
  41
  42 struct pa_resampler {
  43     pa_resample_method_t resample_method;
  44     pa_sample_spec i_ss, o_ss;
  45     pa_channel_map i_cm, o_cm;
  46     size_t i_fz, o_fz;
  47     pa_mempool *mempool;
  48
  49     void (*impl_free)(pa_resampler *r);
  50     void (*impl_update_input_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     assert(pool);
  86     assert(a);
  87     assert(b);
  88     assert(pa_sample_spec_valid(a));
  89     assert(pa_sample_spec_valid(b));
  90     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     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     assert(r);
 157     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_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) {
 169     assert(r && in && out && r->impl_run);
 170
 171     r->impl_run(r, in, out);
 172 }
 173
 174 size_t pa_resampler_request(pa_resampler *r, size_t out_length) {
 175     assert(r);
 176
 177     return (((out_length / r->o_fz)*r->i_ss.rate)/r->o_ss.rate) * r->i_fz;
 178 }
 179
 180 pa_resample_method_t pa_resampler_get_method(pa_resampler *r) {
 181     assert(r);
 182     return r->resample_method;
 183 }
 184
 185 static const char * const resample_methods[] = {
 186     "src-sinc-best-quality",
 187     "src-sinc-medium-quality",
 188     "src-sinc-fastest",
 189     "src-zero-order-hold",
 190     "src-linear",
 191     "trivial"
 192 };
 193
 194 const char *pa_resample_method_to_string(pa_resample_method_t m) {
 195
 196     if (m < 0 || m >= PA_RESAMPLER_MAX)
 197         return NULL;
 198
 199     return resample_methods[m];
 200 }
 201
 202 pa_resample_method_t pa_parse_resample_method(const char *string) {
 203     pa_resample_method_t m;
 204
 205     assert(string);
 206
 207     for (m = 0; m < PA_RESAMPLER_MAX; m++)
 208         if (!strcmp(string, resample_methods[m]))
 209             return m;
 210
 211     return PA_RESAMPLER_INVALID;
 212 }
 213
 214
 215 /*** libsamplerate based implementation ***/
 216
 217 static void libsamplerate_free(pa_resampler *r) {
 218     struct impl_libsamplerate *u;
 219
 220     assert(r);
 221     assert(r->impl_data);
 222
 223     u = r->impl_data;
 224
 225     if (u->src_state)
 226         src_delete(u->src_state);
 227
 228     if (u->buf1.memblock)
 229         pa_memblock_unref(u->buf1.memblock);
 230     if (u->buf2.memblock)
 231         pa_memblock_unref(u->buf2.memblock);
 232     if (u->buf3.memblock)
 233         pa_memblock_unref(u->buf3.memblock);
 234     if (u->buf4.memblock)
 235         pa_memblock_unref(u->buf4.memblock);
 236     pa_xfree(u);
 237 }
 238
 239 static void calc_map_table(pa_resampler *r) {
 240     struct impl_libsamplerate *u;
 241     unsigned oc;
 242     assert(r);
 243     assert(r->impl_data);
 244
 245     u = r->impl_data;
 246
 247     if (!(u->map_required = (!pa_channel_map_equal(&r->i_cm, &r->o_cm) || r->i_ss.channels != r->o_ss.channels)))
 248         return;
 249
 250     for (oc = 0; oc < r->o_ss.channels; oc++) {
 251         unsigned ic, i = 0;
 252
 253         for (ic = 0; ic < r->i_ss.channels; ic++) {
 254             pa_channel_position_t a, b;
 255
 256             a = r->i_cm.map[ic];
 257             b = r->o_cm.map[oc];
 258
 259             if (a == b ||
 260                 (a == PA_CHANNEL_POSITION_MONO && b == PA_CHANNEL_POSITION_LEFT) ||
 261                 (a == PA_CHANNEL_POSITION_MONO && b == PA_CHANNEL_POSITION_RIGHT) ||
 262                 (a == PA_CHANNEL_POSITION_LEFT && b == PA_CHANNEL_POSITION_MONO) ||
 263                 (a == PA_CHANNEL_POSITION_RIGHT && b == PA_CHANNEL_POSITION_MONO))
 264
 265                 u->map_table[oc][i++] = ic;
 266         }
 267
 268         /* Add an end marker */
 269         if (i < PA_CHANNELS_MAX)
 270             u->map_table[oc][i] = -1;
 271     }
 272 }
 273
 274 static pa_memchunk* convert_to_float(pa_resampler *r, pa_memchunk *input) {
 275     struct impl_libsamplerate *u;
 276     unsigned n_samples;
 277     void *src, *dst;
 278
 279     assert(r);
 280     assert(input);
 281     assert(input->memblock);
 282
 283     assert(r->impl_data);
 284     u = r->impl_data;
 285
 286     /* Convert the incoming sample into floats and place them in buf1 */
 287
 288     if (!u->to_float32ne_func || !input->length)
 289         return input;
 290
 291     n_samples = (input->length / r->i_fz) * r->i_ss.channels;
 292
 293     if (!u->buf1.memblock || u->buf1_samples < n_samples) {
 294         if (u->buf1.memblock)
 295             pa_memblock_unref(u->buf1.memblock);
 296
 297         u->buf1_samples = n_samples;
 298         u->buf1.memblock = pa_memblock_new(r->mempool, u->buf1.length = sizeof(float) * n_samples);
 299         u->buf1.index = 0;
 300     }
 301
 302     src = (uint8_t*) pa_memblock_acquire(input->memblock) + input->index;
 303     dst = (uint8_t*) pa_memblock_acquire(u->buf1.memblock);
 304     u->to_float32ne_func(n_samples, src, dst);
 305     pa_memblock_release(input->memblock);
 306     pa_memblock_release(u->buf1.memblock);
 307
 308     u->buf1.length = sizeof(float) * n_samples;
 309
 310     return &u->buf1;
 311 }
 312
 313 static pa_memchunk *remap_channels(pa_resampler *r, pa_memchunk *input) {
 314     struct impl_libsamplerate *u;
 315     unsigned n_samples, n_frames;
 316     int i_skip, o_skip;
 317     unsigned oc;
 318     float *src, *dst;
 319
 320     assert(r);
 321     assert(input);
 322     assert(input->memblock);
 323
 324     assert(r->impl_data);
 325     u = r->impl_data;
 326
 327     /* Remap channels and place the result int buf2 */
 328
 329     if (!u->map_required || !input->length)
 330         return input;
 331
 332     n_samples = input->length / sizeof(float);
 333     n_frames = n_samples / r->o_ss.channels;
 334
 335     if (!u->buf2.memblock || u->buf2_samples < n_samples) {
 336         if (u->buf2.memblock)
 337             pa_memblock_unref(u->buf2.memblock);
 338
 339         u->buf2_samples = n_samples;
 340         u->buf2.memblock = pa_memblock_new(r->mempool, u->buf2.length = sizeof(float) * n_samples);
 341         u->buf2.index = 0;
 342     }
 343
 344     src = (float*) ((uint8_t*) pa_memblock_acquire(input->memblock) + input->index);
 345     dst = (float*) pa_memblock_acquire(u->buf2.memblock);
 346
 347     memset(dst, 0, n_samples * sizeof(float));
 348
 349     o_skip = sizeof(float) * r->o_ss.channels;
 350     i_skip = sizeof(float) * r->i_ss.channels;
 351
 352     for (oc = 0; oc < r->o_ss.channels; oc++) {
 353         unsigned i;
 354         static const float one = 1.0;
 355
 356         for (i = 0; i < PA_CHANNELS_MAX && u->map_table[oc][i] >= 0; i++)
 357             oil_vectoradd_f32(
 358                 dst + oc, o_skip,
 359                 dst + oc, o_skip,
 360                 src + u->map_table[oc][i], i_skip,
 361                 n_frames,
 362                 &one, &one);
 363     }
 364
 365     pa_memblock_release(input->memblock);
 366     pa_memblock_release(u->buf2.memblock);
 367
 368     u->buf2.length = n_frames * sizeof(float) * r->o_ss.channels;
 369
 370     return &u->buf2;
 371 }
 372
 373 static pa_memchunk *resample(pa_resampler *r, pa_memchunk *input) {
 374     struct impl_libsamplerate *u;
 375     SRC_DATA data;
 376     unsigned in_n_frames, in_n_samples;
 377     unsigned out_n_frames, out_n_samples;
 378     int ret;
 379
 380     assert(r);
 381     assert(input);
 382     assert(r->impl_data);
 383     u = r->impl_data;
 384
 385     /* Resample the data and place the result in buf3 */
 386
 387     if (!u->src_state || !input->length)
 388         return input;
 389
 390     in_n_samples = input->length / sizeof(float);
 391     in_n_frames = in_n_samples * r->o_ss.channels;
 392
 393     out_n_frames = (in_n_frames*r->o_ss.rate/r->i_ss.rate)+1024;
 394     out_n_samples = out_n_frames * r->o_ss.channels;
 395
 396     if (!u->buf3.memblock || u->buf3_samples < out_n_samples) {
 397         if (u->buf3.memblock)
 398             pa_memblock_unref(u->buf3.memblock);
 399
 400         u->buf3_samples = out_n_samples;
 401         u->buf3.memblock = pa_memblock_new(r->mempool, u->buf3.length = sizeof(float) * out_n_samples);
 402         u->buf3.index = 0;
 403     }
 404
 405     data.data_in = (float*) ((uint8_t*) pa_memblock_acquire(input->memblock) + input->index);
 406     data.input_frames = in_n_frames;
 407
 408     data.data_out = (float*) pa_memblock_acquire(u->buf3.memblock);
 409     data.output_frames = out_n_frames;
 410
 411     data.src_ratio = (double) r->o_ss.rate / r->i_ss.rate;
 412     data.end_of_input = 0;
 413
 414     ret = src_process(u->src_state, &data);
 415     assert(ret == 0);
 416     assert((unsigned) data.input_frames_used == in_n_frames);
 417
 418     pa_memblock_release(input->memblock);
 419     pa_memblock_release(u->buf3.memblock);
 420
 421     u->buf3.length = data.output_frames_gen * sizeof(float) * r->o_ss.channels;
 422
 423     return &u->buf3;
 424 }
 425
 426 static pa_memchunk *convert_from_float(pa_resampler *r, pa_memchunk *input) {
 427     struct impl_libsamplerate *u;
 428     unsigned n_samples, n_frames;
 429     void *src, *dst;
 430
 431     assert(r);
 432     assert(input);
 433     assert(r->impl_data);
 434     u = r->impl_data;
 435
 436     /* Convert the data into the correct sample type and place the result in buf4 */
 437
 438     if (!u->from_float32ne_func || !input->length)
 439         return input;
 440
 441     n_frames = input->length / sizeof(float) / r->o_ss.channels;
 442     n_samples = n_frames * r->o_ss.channels;
 443
 444     if (u->buf4_samples < n_samples) {
 445         if (u->buf4.memblock)
 446             pa_memblock_unref(u->buf4.memblock);
 447
 448         u->buf4_samples = n_samples;
 449         u->buf4.memblock = pa_memblock_new(r->mempool, u->buf4.length = r->o_fz * n_frames);
 450         u->buf4.index = 0;
 451     }
 452
 453     src = (uint8_t*) pa_memblock_acquire(input->memblock) + input->length;
 454     dst = pa_memblock_acquire(u->buf4.memblock);
 455     u->from_float32ne_func(n_samples, src, dst);
 456     pa_memblock_release(input->memblock);
 457     pa_memblock_release(u->buf4.memblock);
 458
 459     u->buf4.length = r->o_fz * n_frames;
 460
 461     return &u->buf4;
 462 }
 463
 464 static void libsamplerate_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) {
 465     struct impl_libsamplerate *u;
 466     pa_memchunk *buf;
 467
 468     assert(r);
 469     assert(in);
 470     assert(out);
 471     assert(in->length);
 472     assert(in->memblock);
 473     assert(in->length % r->i_fz == 0);
 474     assert(r->impl_data);
 475
 476     u = r->impl_data;
 477
 478     buf = convert_to_float(r, (pa_memchunk*) in);
 479     buf = remap_channels(r, buf);
 480     buf = resample(r, buf);
 481
 482     if (buf->length) {
 483         buf = convert_from_float(r, buf);
 484         *out = *buf;
 485
 486         if (buf == in)
 487             pa_memblock_ref(buf->memblock);
 488         else
 489             pa_memchunk_reset(buf);
 490     } else
 491         pa_memchunk_reset(out);
 492
 493     pa_memblock_release(in->memblock);
 494
 495 }
 496
 497 static void libsamplerate_update_input_rate(pa_resampler *r, uint32_t rate) {
 498     struct impl_libsamplerate *u;
 499
 500     assert(r);
 501     assert(rate > 0);
 502     assert(r->impl_data);
 503     u = r->impl_data;
 504
 505     if (!u->src_state) {
 506         int err;
 507         u->src_state = src_new(r->resample_method, r->o_ss.channels, &err);
 508         assert(u->src_state);
 509     } else {
 510         int ret = src_set_ratio(u->src_state, (double) r->o_ss.rate / rate);
 511         assert(ret == 0);
 512     }
 513 }
 514
 515 static int libsamplerate_init(pa_resampler *r) {
 516     struct impl_libsamplerate *u = NULL;
 517     int err;
 518
 519     r->impl_data = u = pa_xnew(struct impl_libsamplerate, 1);
 520
 521     pa_memchunk_reset(&u->buf1);
 522     pa_memchunk_reset(&u->buf2);
 523     pa_memchunk_reset(&u->buf3);
 524     pa_memchunk_reset(&u->buf4);
 525     u->buf1_samples = u->buf2_samples = u->buf3_samples = u->buf4_samples = 0;
 526
 527     if (r->i_ss.format == PA_SAMPLE_FLOAT32NE)
 528         u->to_float32ne_func = NULL;
 529     else if (!(u->to_float32ne_func = pa_get_convert_to_float32ne_function(r->i_ss.format)))
 530         goto fail;
 531
 532     if (r->o_ss.format == PA_SAMPLE_FLOAT32NE)
 533         u->from_float32ne_func = NULL;
 534     else if (!(u->from_float32ne_func = pa_get_convert_from_float32ne_function(r->o_ss.format)))
 535         goto fail;
 536
 537     if (r->o_ss.rate == r->i_ss.rate)
 538         u->src_state = NULL;
 539     else if (!(u->src_state = src_new(r->resample_method, r->o_ss.channels, &err)))
 540         goto fail;
 541
 542     r->impl_free = libsamplerate_free;
 543     r->impl_update_input_rate = libsamplerate_update_input_rate;
 544     r->impl_run = libsamplerate_run;
 545
 546     calc_map_table(r);
 547
 548     return 0;
 549
 550 fail:
 551     pa_xfree(u);
 552     return -1;
 553 }
 554
 555 /* Trivial implementation */
 556
 557 static void trivial_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) {
 558     size_t fz;
 559     unsigned  n_frames;
 560     struct impl_trivial *u;
 561
 562     assert(r);
 563     assert(in);
 564     assert(out);
 565     assert(r->impl_data);
 566
 567     u = r->impl_data;
 568
 569     fz = r->i_fz;
 570     assert(fz == r->o_fz);
 571
 572     n_frames = in->length/fz;
 573
 574     if (r->i_ss.rate == r->o_ss.rate) {
 575
 576         /* In case there's no diefference in sample types, do nothing */
 577         *out = *in;
 578         pa_memblock_ref(out->memblock);
 579
 580         u->o_counter += n_frames;
 581     } else {
 582         /* Do real resampling */
 583         size_t l;
 584         unsigned o_index;
 585         void *src, *dst;
 586
 587         /* The length of the new memory block rounded up */
 588         l = ((((n_frames+1) * r->o_ss.rate) / r->i_ss.rate) + 1) * fz;
 589
 590         out->index = 0;
 591         out->memblock = pa_memblock_new(r->mempool, l);
 592
 593         src = (uint8_t*) pa_memblock_acquire(in->memblock) + in->index;
 594         dst = pa_memblock_acquire(out->memblock);
 595
 596         for (o_index = 0;; o_index++, u->o_counter++) {
 597             unsigned j;
 598
 599             j = (u->o_counter * r->i_ss.rate / r->o_ss.rate);
 600             j = j > u->i_counter ? j - u->i_counter : 0;
 601
 602             if (j >= n_frames)
 603                 break;
 604
 605             assert(o_index*fz < pa_memblock_get_length(out->memblock));
 606
 607             memcpy((uint8_t*) dst + fz*o_index,
 608                    (uint8_t*) src + fz*j, fz);
 609
 610         }
 611
 612         pa_memblock_release(in->memblock);
 613         pa_memblock_release(out->memblock);
 614
 615         out->length = o_index*fz;
 616     }
 617
 618     u->i_counter += n_frames;
 619
 620     /* Normalize counters */
 621     while (u->i_counter >= r->i_ss.rate) {
 622         u->i_counter -= r->i_ss.rate;
 623         assert(u->o_counter >= r->o_ss.rate);
 624         u->o_counter -= r->o_ss.rate;
 625     }
 626 }
 627
 628 static void trivial_free(pa_resampler *r) {
 629     assert(r);
 630
 631     pa_xfree(r->impl_data);
 632 }
 633
 634 static void trivial_update_input_rate(pa_resampler *r, uint32_t rate) {
 635     struct impl_trivial *u;
 636
 637     assert(r);
 638     assert(rate > 0);
 639     assert(r->impl_data);
 640
 641     u = r->impl_data;
 642     u->i_counter = 0;
 643     u->o_counter = 0;
 644 }
 645
 646 static int trivial_init(pa_resampler*r) {
 647     struct impl_trivial *u;
 648
 649     assert(r);
 650     assert(r->i_ss.format == r->o_ss.format);
 651     assert(r->i_ss.channels == r->o_ss.channels);
 652
 653     r->impl_data = u = pa_xnew(struct impl_trivial, 1);
 654     u->o_counter = u->i_counter = 0;
 655
 656     r->impl_run = trivial_run;
 657     r->impl_free = trivial_free;
 658     r->impl_update_input_rate = trivial_update_input_rate;
 659
 660     return 0;
 661 }
 662
 663