code.delx.au - pulseaudio/blob - 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_memchunk buf1, buf2, buf3, buf4;
  55     unsigned buf1_samples, buf2_samples, buf3_samples, buf4_samples;
  56
  57     pa_convert_to_float32ne_func_t to_float32ne_func;
  58     pa_convert_from_float32ne_func_t from_float32ne_func;
  59     SRC_STATE *src_state;
  60
  61     int map_table[PA_CHANNELS_MAX][PA_CHANNELS_MAX];
  62     int map_required;
  63 };
  64
  65 struct impl_trivial {
  66     unsigned o_counter;
  67     unsigned i_counter;
  68 };
  69
  70 static int libsamplerate_init(pa_resampler*r);
  71 static int trivial_init(pa_resampler*r);
  72
  73 pa_resampler* pa_resampler_new(
  74         pa_mempool *pool,
  75         const pa_sample_spec *a,
  76         const pa_channel_map *am,
  77         const pa_sample_spec *b,
  78         const pa_channel_map *bm,
  79         pa_resample_method_t resample_method) {
  80
  81     pa_resampler *r = NULL;
  82
  83     assert(pool);
  84     assert(a);
  85     assert(b);
  86     assert(pa_sample_spec_valid(a));
  87     assert(pa_sample_spec_valid(b));
  88     assert(resample_method != PA_RESAMPLER_INVALID);
  89
  90     r = pa_xnew(pa_resampler, 1);
  91     r->impl_data = NULL;
  92     r->mempool = pool;
  93     r->resample_method = resample_method;
  94
  95     r->impl_free = NULL;
  96     r->impl_update_input_rate = NULL;
  97     r->impl_run = NULL;
  98
  99     /* Fill sample specs */
 100     r->i_ss = *a;
 101     r->o_ss = *b;
 102
 103     if (am)
 104         r->i_cm = *am;
 105     else
 106         pa_channel_map_init_auto(&r->i_cm, r->i_ss.channels, PA_CHANNEL_MAP_DEFAULT);
 107
 108     if (bm)
 109         r->o_cm = *bm;
 110     else
 111         pa_channel_map_init_auto(&r->o_cm, r->o_ss.channels, PA_CHANNEL_MAP_DEFAULT);
 112
 113     r->i_fz = pa_frame_size(a);
 114     r->o_fz = pa_frame_size(b);
 115
 116     /* Choose implementation */
 117     if (a->channels != b->channels ||
 118         a->format != b->format ||
 119         !pa_channel_map_equal(&r->i_cm, &r->o_cm) ||
 120         resample_method != PA_RESAMPLER_TRIVIAL) {
 121
 122         /* Use the libsamplerate based resampler for the complicated cases */
 123         if (resample_method == PA_RESAMPLER_TRIVIAL)
 124             r->resample_method = PA_RESAMPLER_SRC_ZERO_ORDER_HOLD;
 125
 126         if (libsamplerate_init(r) < 0)
 127             goto fail;
 128
 129     } else {
 130         /* Use our own simple non-fp resampler for the trivial cases and when the user selects it */
 131         if (trivial_init(r) < 0)
 132             goto fail;
 133     }
 134
 135     return r;
 136
 137 fail:
 138     if (r)
 139         pa_xfree(r);
 140
 141     return NULL;
 142 }
 143
 144 void pa_resampler_free(pa_resampler *r) {
 145     assert(r);
 146
 147     if (r->impl_free)
 148         r->impl_free(r);
 149
 150     pa_xfree(r);
 151 }
 152
 153 void pa_resampler_set_input_rate(pa_resampler *r, uint32_t rate) {
 154     assert(r);
 155     assert(rate > 0);
 156
 157     if (r->i_ss.rate == rate)
 158         return;
 159
 160     r->i_ss.rate = rate;
 161
 162     if (r->impl_update_input_rate)
 163         r->impl_update_input_rate(r, rate);
 164 }
 165
 166 void pa_resampler_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) {
 167     assert(r && in && out && r->impl_run);
 168
 169     r->impl_run(r, in, out);
 170 }
 171
 172 size_t pa_resampler_request(pa_resampler *r, size_t out_length) {
 173     assert(r);
 174
 175     return (((out_length / r->o_fz)*r->i_ss.rate)/r->o_ss.rate) * r->i_fz;
 176 }
 177
 178 pa_resample_method_t pa_resampler_get_method(pa_resampler *r) {
 179     assert(r);
 180     return r->resample_method;
 181 }
 182
 183 static const char * const resample_methods[] = {
 184     "src-sinc-best-quality",
 185     "src-sinc-medium-quality",
 186     "src-sinc-fastest",
 187     "src-zero-order-hold",
 188     "src-linear",
 189     "trivial"
 190 };
 191
 192 const char *pa_resample_method_to_string(pa_resample_method_t m) {
 193
 194     if (m < 0 || m >= PA_RESAMPLER_MAX)
 195         return NULL;
 196
 197     return resample_methods[m];
 198 }
 199
 200 pa_resample_method_t pa_parse_resample_method(const char *string) {
 201     pa_resample_method_t m;
 202
 203     assert(string);
 204
 205     for (m = 0; m < PA_RESAMPLER_MAX; m++)
 206         if (!strcmp(string, resample_methods[m]))
 207             return m;
 208
 209     return PA_RESAMPLER_INVALID;
 210 }
 211
 212
 213 /*** libsamplerate based implementation ***/
 214
 215 static void libsamplerate_free(pa_resampler *r) {
 216     struct impl_libsamplerate *u;
 217
 218     assert(r);
 219     assert(r->impl_data);
 220
 221     u = r->impl_data;
 222
 223     if (u->src_state)
 224         src_delete(u->src_state);
 225
 226     if (u->buf1.memblock)
 227         pa_memblock_unref(u->buf1.memblock);
 228     if (u->buf2.memblock)
 229         pa_memblock_unref(u->buf2.memblock);
 230     if (u->buf3.memblock)
 231         pa_memblock_unref(u->buf3.memblock);
 232     if (u->buf4.memblock)
 233         pa_memblock_unref(u->buf4.memblock);
 234     pa_xfree(u);
 235 }
 236
 237 static void calc_map_table(pa_resampler *r) {
 238     struct impl_libsamplerate *u;
 239     unsigned oc;
 240     assert(r);
 241     assert(r->impl_data);
 242
 243     u = r->impl_data;
 244
 245     if (!(u->map_required = (!pa_channel_map_equal(&r->i_cm, &r->o_cm) || r->i_ss.channels != r->o_ss.channels)))
 246         return;
 247
 248     for (oc = 0; oc < r->o_ss.channels; oc++) {
 249         unsigned ic, i = 0;
 250
 251         for (ic = 0; ic < r->i_ss.channels; ic++) {
 252             pa_channel_position_t a, b;
 253
 254             a = r->i_cm.map[ic];
 255             b = r->o_cm.map[oc];
 256
 257             if (a == b ||
 258                 (a == PA_CHANNEL_POSITION_MONO && b == PA_CHANNEL_POSITION_LEFT) ||
 259                 (a == PA_CHANNEL_POSITION_MONO && b == PA_CHANNEL_POSITION_RIGHT) ||
 260                 (a == PA_CHANNEL_POSITION_LEFT && b == PA_CHANNEL_POSITION_MONO) ||
 261                 (a == PA_CHANNEL_POSITION_RIGHT && b == PA_CHANNEL_POSITION_MONO))
 262
 263                 u->map_table[oc][i++] = ic;
 264         }
 265
 266         /* Add an end marker */
 267         if (i < PA_CHANNELS_MAX)
 268             u->map_table[oc][i] = -1;
 269     }
 270 }
 271
 272 static pa_memchunk* convert_to_float(pa_resampler *r, pa_memchunk *input) {
 273     struct impl_libsamplerate *u;
 274     unsigned n_samples;
 275     void *src, *dst;
 276
 277     assert(r);
 278     assert(input);
 279     assert(input->memblock);
 280
 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 || !input->length)
 287         return input;
 288
 289     n_samples = (input->length / r->i_fz) * r->i_ss.channels;
 290
 291     if (!u->buf1.memblock || u->buf1_samples < n_samples) {
 292         if (u->buf1.memblock)
 293             pa_memblock_unref(u->buf1.memblock);
 294
 295         u->buf1_samples = n_samples;
 296         u->buf1.memblock = pa_memblock_new(r->mempool, u->buf1.length = sizeof(float) * n_samples);
 297         u->buf1.index = 0;
 298     }
 299
 300     src = (uint8_t*) pa_memblock_acquire(input->memblock) + input->index;
 301     dst = (uint8_t*) pa_memblock_acquire(u->buf1.memblock);
 302     u->to_float32ne_func(n_samples, src, dst);
 303     pa_memblock_release(input->memblock);
 304     pa_memblock_release(u->buf1.memblock);
 305
 306     u->buf1.length = sizeof(float) * n_samples;
 307
 308     return &u->buf1;
 309 }
 310
 311 static pa_memchunk *remap_channels(pa_resampler *r, pa_memchunk *input) {
 312     struct impl_libsamplerate *u;
 313     unsigned n_samples, n_frames;
 314     int i_skip, o_skip;
 315     unsigned oc;
 316     float *src, *dst;
 317
 318     assert(r);
 319     assert(input);
 320     assert(input->memblock);
 321
 322     assert(r->impl_data);
 323     u = r->impl_data;
 324
 325     /* Remap channels and place the result int buf2 */
 326
 327     if (!u->map_required || !input->length)
 328         return input;
 329
 330     n_samples = input->length / sizeof(float);
 331     n_frames = n_samples / r->o_ss.channels;
 332
 333     if (!u->buf2.memblock || u->buf2_samples < n_samples) {
 334         if (u->buf2.memblock)
 335             pa_memblock_unref(u->buf2.memblock);
 336
 337         u->buf2_samples = n_samples;
 338         u->buf2.memblock = pa_memblock_new(r->mempool, u->buf2.length = sizeof(float) * n_samples);
 339         u->buf2.index = 0;
 340     }
 341
 342     src = (float*) ((uint8_t*) pa_memblock_acquire(input->memblock) + input->index);
 343     dst = (float*) pa_memblock_acquire(u->buf2.memblock);
 344
 345     memset(dst, 0, n_samples * sizeof(float));
 346
 347     o_skip = sizeof(float) * r->o_ss.channels;
 348     i_skip = sizeof(float) * r->i_ss.channels;
 349
 350     for (oc = 0; oc < r->o_ss.channels; oc++) {
 351         unsigned i;
 352         static const float one = 1.0;
 353
 354         for (i = 0; i < PA_CHANNELS_MAX && u->map_table[oc][i] >= 0; i++)
 355             oil_vectoradd_f32(
 356                 dst + oc, o_skip,
 357                 dst + oc, o_skip,
 358                 src + u->map_table[oc][i], i_skip,
 359                 n_frames,
 360                 &one, &one);
 361     }
 362
 363     pa_memblock_release(input->memblock);
 364     pa_memblock_release(u->buf2.memblock);
 365
 366     u->buf2.length = n_frames * sizeof(float) * r->o_ss.channels;
 367
 368     return &u->buf2;
 369 }
 370
 371 static pa_memchunk *resample(pa_resampler *r, pa_memchunk *input) {
 372     struct impl_libsamplerate *u;
 373     SRC_DATA data;
 374     unsigned in_n_frames, in_n_samples;
 375     unsigned out_n_frames, out_n_samples;
 376     int ret;
 377
 378     assert(r);
 379     assert(input);
 380     assert(r->impl_data);
 381     u = r->impl_data;
 382
 383     /* Resample the data and place the result in buf3 */
 384
 385     if (!u->src_state || !input->length)
 386         return input;
 387
 388     in_n_samples = input->length / sizeof(float);
 389     in_n_frames = in_n_samples * r->o_ss.channels;
 390
 391     out_n_frames = (in_n_frames*r->o_ss.rate/r->i_ss.rate)+1024;
 392     out_n_samples = out_n_frames * r->o_ss.channels;
 393
 394     if (!u->buf3.memblock || u->buf3_samples < out_n_samples) {
 395         if (u->buf3.memblock)
 396             pa_memblock_unref(u->buf3.memblock);
 397
 398         u->buf3_samples = out_n_samples;
 399         u->buf3.memblock = pa_memblock_new(r->mempool, u->buf3.length = sizeof(float) * out_n_samples);
 400         u->buf3.index = 0;
 401     }
 402
 403     data.data_in = (float*) ((uint8_t*) pa_memblock_acquire(input->memblock) + input->index);
 404     data.input_frames = in_n_frames;
 405
 406     data.data_out = (float*) pa_memblock_acquire(u->buf3.memblock);
 407     data.output_frames = out_n_frames;
 408
 409     data.src_ratio = (double) r->o_ss.rate / r->i_ss.rate;
 410     data.end_of_input = 0;
 411
 412     ret = src_process(u->src_state, &data);
 413     assert(ret == 0);
 414     assert((unsigned) data.input_frames_used == in_n_frames);
 415
 416     pa_memblock_release(input->memblock);
 417     pa_memblock_release(u->buf3.memblock);
 418
 419     u->buf3.length = data.output_frames_gen * sizeof(float) * r->o_ss.channels;
 420
 421     return &u->buf3;
 422 }
 423
 424 static pa_memchunk *convert_from_float(pa_resampler *r, pa_memchunk *input) {
 425     struct impl_libsamplerate *u;
 426     unsigned n_samples, n_frames;
 427     void *src, *dst;
 428
 429     assert(r);
 430     assert(input);
 431     assert(r->impl_data);
 432     u = r->impl_data;
 433
 434     /* Convert the data into the correct sample type and place the result in buf4 */
 435
 436     if (!u->from_float32ne_func || !input->length)
 437         return input;
 438
 439     n_frames = input->length / sizeof(float) / r->o_ss.channels;
 440     n_samples = n_frames * r->o_ss.channels;
 441
 442     if (u->buf4_samples < n_samples) {
 443         if (u->buf4.memblock)
 444             pa_memblock_unref(u->buf4.memblock);
 445
 446         u->buf4_samples = n_samples;
 447         u->buf4.memblock = pa_memblock_new(r->mempool, u->buf4.length = r->o_fz * n_frames);
 448         u->buf4.index = 0;
 449     }
 450
 451     src = (uint8_t*) pa_memblock_acquire(input->memblock) + input->length;
 452     dst = pa_memblock_acquire(u->buf4.memblock);
 453     u->from_float32ne_func(n_samples, src, dst);
 454     pa_memblock_release(input->memblock);
 455     pa_memblock_release(u->buf4.memblock);
 456
 457     u->buf4.length = r->o_fz * n_frames;
 458
 459     return &u->buf4;
 460 }
 461
 462 static void libsamplerate_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) {
 463     struct impl_libsamplerate *u;
 464     pa_memchunk *buf;
 465
 466     assert(r);
 467     assert(in);
 468     assert(out);
 469     assert(in->length);
 470     assert(in->memblock);
 471     assert(in->length % r->i_fz == 0);
 472     assert(r->impl_data);
 473
 474     u = r->impl_data;
 475
 476     buf = convert_to_float(r, (pa_memchunk*) in);
 477     buf = remap_channels(r, buf);
 478     buf = resample(r, buf);
 479
 480     if (buf->length) {
 481         buf = convert_from_float(r, buf);
 482         *out = *buf;
 483
 484         if (buf == in)
 485             pa_memblock_ref(buf->memblock);
 486         else
 487             pa_memchunk_reset(buf);
 488     } else
 489         pa_memchunk_reset(out);
 490
 491     pa_memblock_release(in->memblock);
 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     pa_memchunk_reset(&u->buf1);
 520     pa_memchunk_reset(&u->buf2);
 521     pa_memchunk_reset(&u->buf3);
 522     pa_memchunk_reset(&u->buf4);
 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         void *src, *dst;
 584
 585         /* The length of the new memory block rounded up */
 586         l = ((((n_frames+1) * r->o_ss.rate) / r->i_ss.rate) + 1) * fz;
 587
 588         out->index = 0;
 589         out->memblock = pa_memblock_new(r->mempool, l);
 590
 591         src = (uint8_t*) pa_memblock_acquire(in->memblock) + in->index;
 592         dst = pa_memblock_acquire(out->memblock);
 593
 594         for (o_index = 0;; o_index++, u->o_counter++) {
 595             unsigned j;
 596
 597             j = (u->o_counter * r->i_ss.rate / r->o_ss.rate);
 598             j = j > u->i_counter ? j - u->i_counter : 0;
 599
 600             if (j >= n_frames)
 601                 break;
 602
 603             assert(o_index*fz < pa_memblock_get_length(out->memblock));
 604
 605             memcpy((uint8_t*) dst + fz*o_index,
 606                    (uint8_t*) src + fz*j, fz);
 607
 608         }
 609
 610         pa_memblock_release(in->memblock);
 611         pa_memblock_release(out->memblock);
 612
 613         out->length = o_index*fz;
 614     }
 615
 616     u->i_counter += n_frames;
 617
 618     /* Normalize counters */
 619     while (u->i_counter >= r->i_ss.rate) {
 620         u->i_counter -= r->i_ss.rate;
 621         assert(u->o_counter >= r->o_ss.rate);
 622         u->o_counter -= r->o_ss.rate;
 623     }
 624 }
 625
 626 static void trivial_free(pa_resampler *r) {
 627     assert(r);
 628
 629     pa_xfree(r->impl_data);
 630 }
 631
 632 static void trivial_update_input_rate(pa_resampler *r, uint32_t rate) {
 633     struct impl_trivial *u;
 634
 635     assert(r);
 636     assert(rate > 0);
 637     assert(r->impl_data);
 638
 639     u = r->impl_data;
 640     u->i_counter = 0;
 641     u->o_counter = 0;
 642 }
 643
 644 static int trivial_init(pa_resampler*r) {
 645     struct impl_trivial *u;
 646
 647     assert(r);
 648     assert(r->i_ss.format == r->o_ss.format);
 649     assert(r->i_ss.channels == r->o_ss.channels);
 650
 651     r->impl_data = u = pa_xnew(struct impl_trivial, 1);
 652     u->o_counter = u->i_counter = 0;
 653
 654     r->impl_run = trivial_run;
 655     r->impl_free = trivial_free;
 656     r->impl_update_input_rate = trivial_update_input_rate;
 657
 658     return 0;
 659 }
 660
 661