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
 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 n_samples, n_frames;
 332     int i_skip, o_skip;
 333     unsigned oc;
 334     float *src, *dst;
 335
 336     assert(r);
 337     assert(input);
 338     assert(input->memblock);
 339
 340     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     n_samples = input->length / sizeof(float);
 349     n_frames = n_samples / r->o_ss.channels;
 350
 351     if (!u->buf2.memblock || u->buf2_samples < n_samples) {
 352         if (u->buf2.memblock)
 353             pa_memblock_unref(u->buf2.memblock);
 354
 355         u->buf2_samples = n_samples;
 356         u->buf2.memblock = pa_memblock_new(r->mempool, u->buf2.length = sizeof(float) * n_samples);
 357         u->buf2.index = 0;
 358     }
 359
 360     src = (float*) ((uint8_t*) pa_memblock_acquire(input->memblock) + input->index);
 361     dst = (float*) pa_memblock_acquire(u->buf2.memblock);
 362
 363     memset(dst, 0, n_samples * sizeof(float));
 364
 365     o_skip = sizeof(float) * r->o_ss.channels;
 366     i_skip = sizeof(float) * r->i_ss.channels;
 367
 368     for (oc = 0; oc < r->o_ss.channels; oc++) {
 369         unsigned i;
 370         static const float one = 1.0;
 371
 372         for (i = 0; i < PA_CHANNELS_MAX && u->map_table[oc][i] >= 0; i++)
 373             oil_vectoradd_f32(
 374                 dst + oc, o_skip,
 375                 dst + oc, o_skip,
 376                 src + u->map_table[oc][i], i_skip,
 377                 n_frames,
 378                 &one, &one);
 379     }
 380
 381     pa_memblock_release(input->memblock);
 382     pa_memblock_release(u->buf2.memblock);
 383
 384     u->buf2.length = n_frames * sizeof(float) * r->o_ss.channels;
 385
 386     return &u->buf2;
 387 }
 388
 389 static pa_memchunk *resample(pa_resampler *r, pa_memchunk *input) {
 390     struct impl_libsamplerate *u;
 391     SRC_DATA data;
 392     unsigned in_n_frames, in_n_samples;
 393     unsigned out_n_frames, out_n_samples;
 394     int ret;
 395
 396     assert(r);
 397     assert(input);
 398     assert(r->impl_data);
 399     u = r->impl_data;
 400
 401     /* Resample the data and place the result in buf3 */
 402
 403     if (!u->src_state || !input->length)
 404         return input;
 405
 406     in_n_samples = input->length / sizeof(float);
 407     in_n_frames = in_n_samples / r->o_ss.channels;
 408
 409     out_n_frames = ((in_n_frames*r->o_ss.rate)/r->i_ss.rate)+1024;
 410     out_n_samples = out_n_frames * r->o_ss.channels;
 411
 412     if (!u->buf3.memblock || u->buf3_samples < out_n_samples) {
 413         if (u->buf3.memblock)
 414             pa_memblock_unref(u->buf3.memblock);
 415
 416         u->buf3_samples = out_n_samples;
 417         u->buf3.memblock = pa_memblock_new(r->mempool, u->buf3.length = sizeof(float) * out_n_samples);
 418         u->buf3.index = 0;
 419     }
 420
 421     data.data_in = (float*) ((uint8_t*) pa_memblock_acquire(input->memblock) + input->index);
 422     data.input_frames = in_n_frames;
 423
 424     data.data_out = (float*) pa_memblock_acquire(u->buf3.memblock);
 425     data.output_frames = out_n_frames;
 426
 427     data.src_ratio = (double) r->o_ss.rate / r->i_ss.rate;
 428     data.end_of_input = 0;
 429
 430     ret = src_process(u->src_state, &data);
 431     assert(ret == 0);
 432     assert((unsigned) data.input_frames_used == in_n_frames);
 433
 434     pa_memblock_release(input->memblock);
 435     pa_memblock_release(u->buf3.memblock);
 436
 437     u->buf3.length = data.output_frames_gen * sizeof(float) * r->o_ss.channels;
 438
 439     return &u->buf3;
 440 }
 441
 442 static pa_memchunk *convert_from_float(pa_resampler *r, pa_memchunk *input) {
 443     struct impl_libsamplerate *u;
 444     unsigned n_samples, n_frames;
 445     void *src, *dst;
 446
 447     assert(r);
 448     assert(input);
 449     assert(r->impl_data);
 450     u = r->impl_data;
 451
 452     /* Convert the data into the correct sample type and place the result in buf4 */
 453
 454     if (!u->from_float32ne_func || !input->length)
 455         return input;
 456
 457     n_frames = input->length / sizeof(float) / r->o_ss.channels;
 458     n_samples = n_frames * r->o_ss.channels;
 459
 460     if (!u->buf4.memblock || u->buf4_samples < n_samples) {
 461         if (u->buf4.memblock)
 462             pa_memblock_unref(u->buf4.memblock);
 463
 464         u->buf4_samples = n_samples;
 465         u->buf4.memblock = pa_memblock_new(r->mempool, u->buf4.length = r->o_fz * n_frames);
 466         u->buf4.index = 0;
 467     }
 468
 469     src = (uint8_t*) pa_memblock_acquire(input->memblock) + input->index;
 470     dst = pa_memblock_acquire(u->buf4.memblock);
 471     u->from_float32ne_func(n_samples, src, dst);
 472     pa_memblock_release(input->memblock);
 473     pa_memblock_release(u->buf4.memblock);
 474
 475     u->buf4.length = r->o_fz * n_frames;
 476
 477     return &u->buf4;
 478 }
 479
 480 static void libsamplerate_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) {
 481     struct impl_libsamplerate *u;
 482     pa_memchunk *buf;
 483
 484     assert(r);
 485     assert(in);
 486     assert(out);
 487     assert(in->length);
 488     assert(in->memblock);
 489     assert(in->length % r->i_fz == 0);
 490     assert(r->impl_data);
 491
 492     u = r->impl_data;
 493
 494     buf = (pa_memchunk*) in;
 495     buf = convert_to_float(r, buf);
 496     buf = remap_channels(r, buf);
 497     buf = resample(r, buf);
 498
 499     if (buf->length) {
 500         buf = convert_from_float(r, buf);
 501         *out = *buf;
 502
 503         if (buf == in)
 504             pa_memblock_ref(buf->memblock);
 505         else
 506             pa_memchunk_reset(buf);
 507     } else
 508         pa_memchunk_reset(out);
 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 static void libsamplerate_update_output_rate(pa_resampler *r, uint32_t rate) {
 530     struct impl_libsamplerate *u;
 531
 532     assert(r);
 533     assert(rate > 0);
 534     assert(r->impl_data);
 535     u = r->impl_data;
 536
 537     if (!u->src_state) {
 538         int err;
 539         u->src_state = src_new(r->resample_method, r->o_ss.channels, &err);
 540         assert(u->src_state);
 541     } else {
 542         int ret = src_set_ratio(u->src_state, (double) rate / r->i_ss.rate);
 543         assert(ret == 0);
 544     }
 545 }
 546
 547 static int libsamplerate_init(pa_resampler *r) {
 548     struct impl_libsamplerate *u = NULL;
 549     int err;
 550
 551     r->impl_data = u = pa_xnew(struct impl_libsamplerate, 1);
 552
 553     pa_memchunk_reset(&u->buf1);
 554     pa_memchunk_reset(&u->buf2);
 555     pa_memchunk_reset(&u->buf3);
 556     pa_memchunk_reset(&u->buf4);
 557     u->buf1_samples = u->buf2_samples = u->buf3_samples = u->buf4_samples = 0;
 558
 559     if (r->i_ss.format == PA_SAMPLE_FLOAT32NE)
 560         u->to_float32ne_func = NULL;
 561     else if (!(u->to_float32ne_func = pa_get_convert_to_float32ne_function(r->i_ss.format)))
 562         goto fail;
 563
 564     if (r->o_ss.format == PA_SAMPLE_FLOAT32NE)
 565         u->from_float32ne_func = NULL;
 566     else if (!(u->from_float32ne_func = pa_get_convert_from_float32ne_function(r->o_ss.format)))
 567         goto fail;
 568
 569     if (r->o_ss.rate == r->i_ss.rate)
 570         u->src_state = NULL;
 571     else if (!(u->src_state = src_new(r->resample_method, r->o_ss.channels, &err)))
 572         goto fail;
 573
 574     r->impl_free = libsamplerate_free;
 575     r->impl_update_input_rate = libsamplerate_update_input_rate;
 576     r->impl_update_output_rate = libsamplerate_update_output_rate;
 577     r->impl_run = libsamplerate_run;
 578
 579     calc_map_table(r);
 580
 581     return 0;
 582
 583 fail:
 584     pa_xfree(u);
 585     return -1;
 586 }
 587
 588 /* Trivial implementation */
 589
 590 static void trivial_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) {
 591     size_t fz;
 592     unsigned  n_frames;
 593     struct impl_trivial *u;
 594
 595     assert(r);
 596     assert(in);
 597     assert(out);
 598     assert(r->impl_data);
 599
 600     u = r->impl_data;
 601
 602     fz = r->i_fz;
 603     assert(fz == r->o_fz);
 604
 605     n_frames = in->length/fz;
 606
 607     if (r->i_ss.rate == r->o_ss.rate) {
 608
 609         /* In case there's no diefference in sample types, do nothing */
 610         *out = *in;
 611         pa_memblock_ref(out->memblock);
 612
 613         u->o_counter += n_frames;
 614     } else {
 615         /* Do real resampling */
 616         size_t l;
 617         unsigned o_index;
 618         void *src, *dst;
 619
 620         /* The length of the new memory block rounded up */
 621         l = ((((n_frames+1) * r->o_ss.rate) / r->i_ss.rate) + 1) * fz;
 622
 623         out->index = 0;
 624         out->memblock = pa_memblock_new(r->mempool, l);
 625
 626         src = (uint8_t*) pa_memblock_acquire(in->memblock) + in->index;
 627         dst = pa_memblock_acquire(out->memblock);
 628
 629         for (o_index = 0;; o_index++, u->o_counter++) {
 630             unsigned j;
 631
 632             j = (u->o_counter * r->i_ss.rate / r->o_ss.rate);
 633             j = j > u->i_counter ? j - u->i_counter : 0;
 634
 635             if (j >= n_frames)
 636                 break;
 637
 638             assert(o_index*fz < pa_memblock_get_length(out->memblock));
 639
 640             memcpy((uint8_t*) dst + fz*o_index,
 641                    (uint8_t*) src + fz*j, fz);
 642
 643         }
 644
 645         pa_memblock_release(in->memblock);
 646         pa_memblock_release(out->memblock);
 647
 648         out->length = o_index*fz;
 649     }
 650
 651     u->i_counter += n_frames;
 652
 653     /* Normalize counters */
 654     while (u->i_counter >= r->i_ss.rate) {
 655         u->i_counter -= r->i_ss.rate;
 656         assert(u->o_counter >= r->o_ss.rate);
 657         u->o_counter -= r->o_ss.rate;
 658     }
 659 }
 660
 661 static void trivial_free(pa_resampler *r) {
 662     assert(r);
 663
 664     pa_xfree(r->impl_data);
 665 }
 666
 667 static void trivial_update_rate(pa_resampler *r, uint32_t rate) {
 668     struct impl_trivial *u;
 669
 670     assert(r);
 671     assert(rate > 0);
 672     assert(r->impl_data);
 673
 674     u = r->impl_data;
 675     u->i_counter = 0;
 676     u->o_counter = 0;
 677 }
 678
 679 static int trivial_init(pa_resampler*r) {
 680     struct impl_trivial *u;
 681
 682     assert(r);
 683     assert(r->i_ss.format == r->o_ss.format);
 684     assert(r->i_ss.channels == r->o_ss.channels);
 685
 686     r->impl_data = u = pa_xnew(struct impl_trivial, 1);
 687     u->o_counter = u->i_counter = 0;
 688
 689     r->impl_run = trivial_run;
 690     r->impl_free = trivial_free;
 691     r->impl_update_input_rate = trivial_update_rate;
 692     r->impl_update_output_rate = trivial_update_rate;
 693
 694     return 0;
 695 }
 696
 697