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