code.delx.au - pulseaudio/blob - 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_update_output_rate)(pa_resampler *r, uint32_t rate);
  52     void (*impl_run)(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out);
  53     void *impl_data;
  54 };
  55
  56 struct impl_libsamplerate {
  57     pa_memchunk buf1, buf2, buf3, buf4;
  58     unsigned buf1_samples, buf2_samples, buf3_samples, buf4_samples;
  59
  60     pa_convert_to_float32ne_func_t to_float32ne_func;
  61     pa_convert_from_float32ne_func_t from_float32ne_func;
  62     SRC_STATE *src_state;
  63
  64     int map_table[PA_CHANNELS_MAX][PA_CHANNELS_MAX];
  65     int map_required;
  66 };
  67
  68 struct impl_trivial {
  69     unsigned o_counter;
  70     unsigned i_counter;
  71 };
  72
  73 static int libsamplerate_init(pa_resampler*r);
  74 static int trivial_init(pa_resampler*r);
  75
  76 pa_resampler* pa_resampler_new(
  77         pa_mempool *pool,
  78         const pa_sample_spec *a,
  79         const pa_channel_map *am,
  80         const pa_sample_spec *b,
  81         const pa_channel_map *bm,
  82         pa_resample_method_t resample_method) {
  83
  84     pa_resampler *r = NULL;
  85
  86     assert(pool);
  87     assert(a);
  88     assert(b);
  89     assert(pa_sample_spec_valid(a));
  90     assert(pa_sample_spec_valid(b));
  91     assert(resample_method != PA_RESAMPLER_INVALID);
  92
  93     r = pa_xnew(pa_resampler, 1);
  94     r->impl_data = NULL;
  95     r->mempool = pool;
  96     r->resample_method = resample_method;
  97
  98     r->impl_free = NULL;
  99     r->impl_update_input_rate = NULL;
 100     r->impl_run = NULL;
 101
 102     /* Fill sample specs */
 103     r->i_ss = *a;
 104     r->o_ss = *b;
 105
 106     if (am)
 107         r->i_cm = *am;
 108     else
 109         pa_channel_map_init_auto(&r->i_cm, r->i_ss.channels, PA_CHANNEL_MAP_DEFAULT);
 110
 111     if (bm)
 112         r->o_cm = *bm;
 113     else
 114         pa_channel_map_init_auto(&r->o_cm, r->o_ss.channels, PA_CHANNEL_MAP_DEFAULT);
 115
 116     r->i_fz = pa_frame_size(a);
 117     r->o_fz = pa_frame_size(b);
 118
 119     /* Choose implementation */
 120     if (a->channels != b->channels ||
 121         a->format != b->format ||
 122         !pa_channel_map_equal(&r->i_cm, &r->o_cm) ||
 123         resample_method != PA_RESAMPLER_TRIVIAL) {
 124
 125         /* Use the libsamplerate based resampler for the complicated cases */
 126         if (resample_method == PA_RESAMPLER_TRIVIAL)
 127             r->resample_method = PA_RESAMPLER_SRC_ZERO_ORDER_HOLD;
 128
 129         if (libsamplerate_init(r) < 0)
 130             goto fail;
 131
 132     } else {
 133         /* Use our own simple non-fp resampler for the trivial cases and when the user selects it */
 134         if (trivial_init(r) < 0)
 135             goto fail;
 136     }
 137
 138     return r;
 139
 140 fail:
 141     if (r)
 142         pa_xfree(r);
 143
 144     return NULL;
 145 }
 146
 147 void pa_resampler_free(pa_resampler *r) {
 148     assert(r);
 149
 150     if (r->impl_free)
 151         r->impl_free(r);
 152
 153     pa_xfree(r);
 154 }
 155
 156 void pa_resampler_set_input_rate(pa_resampler *r, uint32_t rate) {
 157     assert(r);
 158     assert(rate > 0);
 159
 160     if (r->i_ss.rate == rate)
 161         return;
 162
 163     r->i_ss.rate = rate;
 164
 165     if (r->impl_update_input_rate)
 166         r->impl_update_input_rate(r, rate);
 167 }
 168
 169 void pa_resampler_set_output_rate(pa_resampler *r, uint32_t rate) {
 170     assert(r);
 171     assert(rate > 0);
 172
 173     if (r->o_ss.rate == rate)
 174         return;
 175
 176     r->o_ss.rate = rate;
 177
 178     if (r->impl_update_output_rate)
 179         r->impl_update_output_rate(r, rate);
 180 }
 181
 182 void pa_resampler_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) {
 183     assert(r && in && out && r->impl_run);
 184
 185     r->impl_run(r, in, out);
 186 }
 187
 188 size_t pa_resampler_request(pa_resampler *r, size_t out_length) {
 189     assert(r);
 190
 191     return (((out_length / r->o_fz)*r->i_ss.rate)/r->o_ss.rate) * r->i_fz;
 192 }
 193
 194 pa_resample_method_t pa_resampler_get_method(pa_resampler *r) {
 195     assert(r);
 196     return r->resample_method;
 197 }
 198
 199 static const char * const resample_methods[] = {
 200     "src-sinc-best-quality",
 201     "src-sinc-medium-quality",
 202     "src-sinc-fastest",
 203     "src-zero-order-hold",
 204     "src-linear",
 205     "trivial"
 206 };
 207
 208 const char *pa_resample_method_to_string(pa_resample_method_t m) {
 209
 210     if (m < 0 || m >= PA_RESAMPLER_MAX)
 211         return NULL;
 212
 213     return resample_methods[m];
 214 }
 215
 216 pa_resample_method_t pa_parse_resample_method(const char *string) {
 217     pa_resample_method_t m;
 218
 219     assert(string);
 220
 221     for (m = 0; m < PA_RESAMPLER_MAX; m++)
 222         if (!strcmp(string, resample_methods[m]))
 223             return m;
 224
 225     return PA_RESAMPLER_INVALID;
 226 }
 227
 228
 229 /*** libsamplerate based implementation ***/
 230
 231 static void libsamplerate_free(pa_resampler *r) {
 232     struct impl_libsamplerate *u;
 233
 234     assert(r);
 235     assert(r->impl_data);
 236
 237     u = r->impl_data;
 238
 239     if (u->src_state)
 240         src_delete(u->src_state);
 241
 242     if (u->buf1.memblock)
 243         pa_memblock_unref(u->buf1.memblock);
 244     if (u->buf2.memblock)
 245         pa_memblock_unref(u->buf2.memblock);
 246     if (u->buf3.memblock)
 247         pa_memblock_unref(u->buf3.memblock);
 248     if (u->buf4.memblock)
 249         pa_memblock_unref(u->buf4.memblock);
 250     pa_xfree(u);
 251 }
 252
 253 static void calc_map_table(pa_resampler *r) {
 254     struct impl_libsamplerate *u;
 255     unsigned oc;
 256     assert(r);
 257     assert(r->impl_data);
 258
 259     u = r->impl_data;
 260
 261     if (!(u->map_required = (!pa_channel_map_equal(&r->i_cm, &r->o_cm) || r->i_ss.channels != r->o_ss.channels)))
 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     assert(r);
 294     assert(input);
 295     assert(input->memblock);
 296
 297     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     u->to_float32ne_func(n_samples, src, dst);
 319     pa_memblock_release(input->memblock);
 320     pa_memblock_release(u->buf1.memblock);
 321
 322     u->buf1.length = sizeof(float) * n_samples;
 323
 324     return &u->buf1;
 325 }
 326
 327 static pa_memchunk *remap_channels(pa_resampler *r, pa_memchunk *input) {
 328     struct impl_libsamplerate *u;
 329     unsigned n_samples, n_frames;
 330     int i_skip, o_skip;
 331     unsigned oc;
 332     float *src, *dst;
 333
 334     assert(r);
 335     assert(input);
 336     assert(input->memblock);
 337
 338     assert(r->impl_data);
 339     u = r->impl_data;
 340
 341     /* Remap channels and place the result int buf2 */
 342
 343     if (!u->map_required || !input->length)
 344         return input;
 345
 346     n_samples = input->length / sizeof(float);
 347     n_frames = n_samples / r->o_ss.channels;
 348
 349     if (!u->buf2.memblock || u->buf2_samples < n_samples) {
 350         if (u->buf2.memblock)
 351             pa_memblock_unref(u->buf2.memblock);
 352
 353         u->buf2_samples = n_samples;
 354         u->buf2.memblock = pa_memblock_new(r->mempool, u->buf2.length = sizeof(float) * n_samples);
 355         u->buf2.index = 0;
 356     }
 357
 358     src = (float*) ((uint8_t*) pa_memblock_acquire(input->memblock) + input->index);
 359     dst = (float*) pa_memblock_acquire(u->buf2.memblock);
 360
 361     memset(dst, 0, n_samples * sizeof(float));
 362
 363     o_skip = sizeof(float) * r->o_ss.channels;
 364     i_skip = sizeof(float) * r->i_ss.channels;
 365
 366     for (oc = 0; oc < r->o_ss.channels; oc++) {
 367         unsigned i;
 368         static const float one = 1.0;
 369
 370         for (i = 0; i < PA_CHANNELS_MAX && u->map_table[oc][i] >= 0; i++)
 371             oil_vectoradd_f32(
 372                 dst + oc, o_skip,
 373                 dst + oc, o_skip,
 374                 src + u->map_table[oc][i], i_skip,
 375                 n_frames,
 376                 &one, &one);
 377     }
 378
 379     pa_memblock_release(input->memblock);
 380     pa_memblock_release(u->buf2.memblock);
 381
 382     u->buf2.length = n_frames * sizeof(float) * r->o_ss.channels;
 383
 384     return &u->buf2;
 385 }
 386
 387 static pa_memchunk *resample(pa_resampler *r, pa_memchunk *input) {
 388     struct impl_libsamplerate *u;
 389     SRC_DATA data;
 390     unsigned in_n_frames, in_n_samples;
 391     unsigned out_n_frames, out_n_samples;
 392     int ret;
 393
 394     assert(r);
 395     assert(input);
 396     assert(r->impl_data);
 397     u = r->impl_data;
 398
 399     /* Resample the data and place the result in buf3 */
 400
 401     if (!u->src_state || !input->length)
 402         return input;
 403
 404     in_n_samples = input->length / sizeof(float);
 405     in_n_frames = in_n_samples * r->o_ss.channels;
 406
 407     out_n_frames = (in_n_frames*r->o_ss.rate/r->i_ss.rate)+1024;
 408     out_n_samples = out_n_frames * r->o_ss.channels;
 409
 410     if (!u->buf3.memblock || u->buf3_samples < out_n_samples) {
 411         if (u->buf3.memblock)
 412             pa_memblock_unref(u->buf3.memblock);
 413
 414         u->buf3_samples = out_n_samples;
 415         u->buf3.memblock = pa_memblock_new(r->mempool, u->buf3.length = sizeof(float) * out_n_samples);
 416         u->buf3.index = 0;
 417     }
 418
 419     data.data_in = (float*) ((uint8_t*) pa_memblock_acquire(input->memblock) + input->index);
 420     data.input_frames = in_n_frames;
 421
 422     data.data_out = (float*) pa_memblock_acquire(u->buf3.memblock);
 423     data.output_frames = out_n_frames;
 424
 425     data.src_ratio = (double) r->o_ss.rate / r->i_ss.rate;
 426     data.end_of_input = 0;
 427
 428     ret = src_process(u->src_state, &data);
 429     assert(ret == 0);
 430     assert((unsigned) data.input_frames_used == in_n_frames);
 431
 432     pa_memblock_release(input->memblock);
 433     pa_memblock_release(u->buf3.memblock);
 434
 435     u->buf3.length = data.output_frames_gen * sizeof(float) * r->o_ss.channels;
 436
 437     return &u->buf3;
 438 }
 439
 440 static pa_memchunk *convert_from_float(pa_resampler *r, pa_memchunk *input) {
 441     struct impl_libsamplerate *u;
 442     unsigned n_samples, n_frames;
 443     void *src, *dst;
 444
 445     assert(r);
 446     assert(input);
 447     assert(r->impl_data);
 448     u = r->impl_data;
 449
 450     /* Convert the data into the correct sample type and place the result in buf4 */
 451
 452     if (!u->from_float32ne_func || !input->length)
 453         return input;
 454
 455     n_frames = input->length / sizeof(float) / r->o_ss.channels;
 456     n_samples = n_frames * r->o_ss.channels;
 457
 458     if (u->buf4_samples < n_samples) {
 459         if (u->buf4.memblock)
 460             pa_memblock_unref(u->buf4.memblock);
 461
 462         u->buf4_samples = n_samples;
 463         u->buf4.memblock = pa_memblock_new(r->mempool, u->buf4.length = r->o_fz * n_frames);
 464         u->buf4.index = 0;
 465     }
 466
 467     src = (uint8_t*) pa_memblock_acquire(input->memblock) + input->length;
 468     dst = pa_memblock_acquire(u->buf4.memblock);
 469     u->from_float32ne_func(n_samples, src, dst);
 470     pa_memblock_release(input->memblock);
 471     pa_memblock_release(u->buf4.memblock);
 472
 473     u->buf4.length = r->o_fz * n_frames;
 474
 475     return &u->buf4;
 476 }
 477
 478 static void libsamplerate_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) {
 479     struct impl_libsamplerate *u;
 480     pa_memchunk *buf;
 481
 482     assert(r);
 483     assert(in);
 484     assert(out);
 485     assert(in->length);
 486     assert(in->memblock);
 487     assert(in->length % r->i_fz == 0);
 488     assert(r->impl_data);
 489
 490     u = r->impl_data;
 491
 492     buf = convert_to_float(r, (pa_memchunk*) in);
 493     buf = remap_channels(r, buf);
 494     buf = resample(r, buf);
 495
 496     if (buf->length) {
 497         buf = convert_from_float(r, buf);
 498         *out = *buf;
 499
 500         if (buf == in)
 501             pa_memblock_ref(buf->memblock);
 502         else
 503             pa_memchunk_reset(buf);
 504     } else
 505         pa_memchunk_reset(out);
 506
 507     pa_memblock_release(in->memblock);
 508
 509 }
 510
 511 static void libsamplerate_update_input_rate(pa_resampler *r, uint32_t rate) {
 512     struct impl_libsamplerate *u;
 513
 514     assert(r);
 515     assert(rate > 0);
 516     assert(r->impl_data);
 517     u = r->impl_data;
 518
 519     if (!u->src_state) {
 520         int err;
 521         u->src_state = src_new(r->resample_method, r->o_ss.channels, &err);
 522         assert(u->src_state);
 523     } else {
 524         int ret = src_set_ratio(u->src_state, (double) r->o_ss.rate / rate);
 525         assert(ret == 0);
 526     }
 527 }
 528
 529
 530 static void libsamplerate_update_output_rate(pa_resampler *r, uint32_t rate) {
 531     struct impl_libsamplerate *u;
 532
 533     assert(r);
 534     assert(rate > 0);
 535     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         assert(u->src_state);
 542     } else {
 543         int ret = src_set_ratio(u->src_state, (double) rate / r->i_ss.rate);
 544         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     assert(r);
 597     assert(in);
 598     assert(out);
 599     assert(r->impl_data);
 600
 601     u = r->impl_data;
 602
 603     fz = r->i_fz;
 604     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             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         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     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     assert(r);
 672     assert(rate > 0);
 673     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     assert(r);
 684     assert(r->i_ss.format == r->o_ss.format);
 685     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