src/pulsecore/resampler.c

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