src/polypcore/resampler.c

   1 /* $Id$ */
   2
   3 /***
   4   This file is part of polypaudio.
   5
   6   polypaudio 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   polypaudio 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 polypaudio; 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 <polyp/xmalloc.h>
  34
  35 #include <polypcore/sconv.h>
  36 #include <polypcore/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_memblock_stat *memblock_stat;
  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     float* 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     const pa_sample_spec *a,
  75     const pa_channel_map *am,
  76     const pa_sample_spec *b,
  77     const pa_channel_map *bm,
  78     pa_memblock_stat *s,
  79     pa_resample_method_t resample_method) {
  80
  81     pa_resampler *r = NULL;
  82
  83     assert(a);
  84     assert(b);
  85     assert(pa_sample_spec_valid(a));
  86     assert(pa_sample_spec_valid(b));
  87     assert(resample_method != PA_RESAMPLER_INVALID);
  88
  89     r = pa_xnew(pa_resampler, 1);
  90     r->impl_data = NULL;
  91     r->memblock_stat = s;
  92     r->resample_method = resample_method;
  93
  94     r->impl_free = NULL;
  95     r->impl_update_input_rate = NULL;
  96     r->impl_run = NULL;
  97
  98     /* Fill sample specs */
  99     r->i_ss = *a;
 100     r->o_ss = *b;
 101
 102     if (am)
 103         r->i_cm = *am;
 104     else
 105         pa_channel_map_init_auto(&r->i_cm, r->i_ss.channels, PA_CHANNEL_MAP_DEFAULT);
 106
 107     if (bm)
 108         r->o_cm = *bm;
 109     else
 110         pa_channel_map_init_auto(&r->o_cm, r->o_ss.channels, PA_CHANNEL_MAP_DEFAULT);
 111
 112     r->i_fz = pa_frame_size(a);
 113     r->o_fz = pa_frame_size(b);
 114
 115     /* Choose implementation */
 116     if (a->channels != b->channels ||
 117         a->format != b->format ||
 118         !pa_channel_map_equal(&r->i_cm, &r->o_cm) ||
 119         resample_method != PA_RESAMPLER_TRIVIAL) {
 120
 121         /* Use the libsamplerate based resampler for the complicated cases */
 122         if (resample_method == PA_RESAMPLER_TRIVIAL)
 123             r->resample_method = PA_RESAMPLER_SRC_ZERO_ORDER_HOLD;
 124
 125         if (libsamplerate_init(r) < 0)
 126             goto fail;
 127
 128     } else {
 129         /* Use our own simple non-fp resampler for the trivial cases and when the user selects it */
 130         if (trivial_init(r) < 0)
 131             goto fail;
 132     }
 133
 134     return r;
 135
 136 fail:
 137     if (r)
 138         pa_xfree(r);
 139
 140     return NULL;
 141 }
 142
 143 void pa_resampler_free(pa_resampler *r) {
 144     assert(r);
 145
 146     if (r->impl_free)
 147         r->impl_free(r);
 148
 149     pa_xfree(r);
 150 }
 151
 152 void pa_resampler_set_input_rate(pa_resampler *r, uint32_t rate) {
 153     assert(r);
 154     assert(rate > 0);
 155
 156     if (r->i_ss.rate == rate)
 157         return;
 158
 159     r->i_ss.rate = rate;
 160
 161     if (r->impl_update_input_rate)
 162         r->impl_update_input_rate(r, rate);
 163 }
 164
 165 void pa_resampler_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) {
 166     assert(r && in && out && r->impl_run);
 167
 168     r->impl_run(r, in, out);
 169 }
 170
 171 size_t pa_resampler_request(pa_resampler *r, size_t out_length) {
 172     assert(r);
 173
 174     return (((out_length / r->o_fz)*r->i_ss.rate)/r->o_ss.rate) * r->i_fz;
 175 }
 176
 177 pa_resample_method_t pa_resampler_get_method(pa_resampler *r) {
 178     assert(r);
 179     return r->resample_method;
 180 }
 181
 182 static const char * const resample_methods[] = {
 183     "src-sinc-best-quality",
 184     "src-sinc-medium-quality",
 185     "src-sinc-fastest",
 186     "src-zero-order-hold",
 187     "src-linear",
 188     "trivial"
 189 };
 190
 191 const char *pa_resample_method_to_string(pa_resample_method_t m) {
 192
 193     if (m < 0 || m >= PA_RESAMPLER_MAX)
 194         return NULL;
 195
 196     return resample_methods[m];
 197 }
 198
 199 pa_resample_method_t pa_parse_resample_method(const char *string) {
 200     pa_resample_method_t m;
 201
 202     assert(string);
 203
 204     for (m = 0; m < PA_RESAMPLER_MAX; m++)
 205         if (!strcmp(string, resample_methods[m]))
 206             return m;
 207
 208     return PA_RESAMPLER_INVALID;
 209 }
 210
 211
 212 /*** libsamplerate based implementation ***/
 213
 214 static void libsamplerate_free(pa_resampler *r) {
 215     struct impl_libsamplerate *u;
 216
 217     assert(r);
 218     assert(r->impl_data);
 219
 220     u = r->impl_data;
 221
 222     if (u->src_state)
 223         src_delete(u->src_state);
 224
 225     pa_xfree(u->buf1);
 226     pa_xfree(u->buf2);
 227     pa_xfree(u->buf3);
 228     pa_xfree(u->buf4);
 229     pa_xfree(u);
 230 }
 231
 232 static void calc_map_table(pa_resampler *r) {
 233     struct impl_libsamplerate *u;
 234     unsigned oc;
 235     assert(r);
 236     assert(r->impl_data);
 237
 238     u = r->impl_data;
 239
 240     if (!(u->map_required = (!pa_channel_map_equal(&r->i_cm, &r->o_cm) || r->i_ss.channels != r->o_ss.channels)))
 241         return;
 242
 243     for (oc = 0; oc < r->o_ss.channels; oc++) {
 244         unsigned ic, i = 0;
 245
 246         for (ic = 0; ic < r->i_ss.channels; ic++) {
 247             pa_channel_position_t a, b;
 248
 249             a = r->i_cm.map[ic];
 250             b = r->o_cm.map[oc];
 251
 252             if (a == b ||
 253                 (a == PA_CHANNEL_POSITION_MONO && b == PA_CHANNEL_POSITION_LEFT) ||
 254                 (a == PA_CHANNEL_POSITION_MONO && b == PA_CHANNEL_POSITION_RIGHT) ||
 255                 (a == PA_CHANNEL_POSITION_LEFT && b == PA_CHANNEL_POSITION_MONO) ||
 256                 (a == PA_CHANNEL_POSITION_RIGHT && b == PA_CHANNEL_POSITION_MONO))
 257
 258                 u->map_table[oc][i++] = ic;
 259         }
 260
 261         /* Add an end marker */
 262         if (i < PA_CHANNELS_MAX)
 263             u->map_table[oc][i] = -1;
 264     }
 265 }
 266
 267 static float * convert_to_float(pa_resampler *r, void *input, unsigned n_frames) {
 268     struct impl_libsamplerate *u;
 269     unsigned n_samples;
 270
 271     assert(r);
 272     assert(input);
 273     assert(r->impl_data);
 274     u = r->impl_data;
 275
 276     /* Convert the incoming sample into floats and place them in buf1 */
 277
 278     if (!u->to_float32ne_func)
 279         return input;
 280
 281     n_samples = n_frames * r->i_ss.channels;
 282
 283     if (u->buf1_samples < n_samples)
 284         u->buf1 = pa_xrealloc(u->buf1, sizeof(float) * (u->buf1_samples = n_samples));
 285
 286     u->to_float32ne_func(n_samples, input, u->buf1);
 287
 288     return u->buf1;
 289 }
 290
 291 static float *remap_channels(pa_resampler *r, float *input, unsigned n_frames) {
 292     struct impl_libsamplerate *u;
 293     unsigned n_samples;
 294     int i_skip, o_skip;
 295     unsigned oc;
 296
 297     assert(r);
 298     assert(input);
 299     assert(r->impl_data);
 300     u = r->impl_data;
 301
 302     /* Remap channels and place the result int buf2 */
 303
 304     if (!u->map_required)
 305         return input;
 306
 307     n_samples = n_frames * r->o_ss.channels;
 308
 309     if (u->buf2_samples < n_samples)
 310         u->buf2 = pa_xrealloc(u->buf2, sizeof(float) * (u->buf2_samples = n_samples));
 311
 312     memset(u->buf2, 0, n_samples * sizeof(float));
 313
 314     o_skip = sizeof(float) * r->o_ss.channels;
 315     i_skip = sizeof(float) * r->i_ss.channels;
 316
 317     for (oc = 0; oc < r->o_ss.channels; oc++) {
 318         unsigned i;
 319         static const float one = 1.0;
 320
 321         for (i = 0; i < PA_CHANNELS_MAX && u->map_table[oc][i] >= 0; i++)
 322             oil_vectoradd_f32(
 323                 u->buf2 + oc, o_skip,
 324                 u->buf2 + oc, o_skip,
 325                 input + u->map_table[oc][i], i_skip,
 326                 n_frames,
 327                 &one, &one);
 328     }
 329
 330     return u->buf2;
 331 }
 332
 333 static float *resample(pa_resampler *r, float *input, unsigned *n_frames) {
 334     struct impl_libsamplerate *u;
 335     SRC_DATA data;
 336     unsigned out_n_frames, out_n_samples;
 337     int ret;
 338
 339     assert(r);
 340     assert(input);
 341     assert(n_frames);
 342     assert(r->impl_data);
 343     u = r->impl_data;
 344
 345     /* Resample the data and place the result in buf3 */
 346
 347     if (!u->src_state)
 348         return input;
 349
 350     out_n_frames = (*n_frames*r->o_ss.rate/r->i_ss.rate)+1024;
 351     out_n_samples = out_n_frames * r->o_ss.channels;
 352
 353     if (u->buf3_samples < out_n_samples)
 354         u->buf3 = pa_xrealloc(u->buf3, sizeof(float) * (u->buf3_samples = out_n_samples));
 355
 356     data.data_in = input;
 357     data.input_frames = *n_frames;
 358
 359     data.data_out = u->buf3;
 360     data.output_frames = out_n_frames;
 361
 362     data.src_ratio = (double) r->o_ss.rate / r->i_ss.rate;
 363     data.end_of_input = 0;
 364
 365     ret = src_process(u->src_state, &data);
 366     assert(ret == 0);
 367     assert((unsigned) data.input_frames_used == *n_frames);
 368
 369     *n_frames = data.output_frames_gen;
 370
 371     return u->buf3;
 372 }
 373
 374 static void *convert_from_float(pa_resampler *r, float *input, unsigned n_frames) {
 375     struct impl_libsamplerate *u;
 376     unsigned n_samples;
 377
 378     assert(r);
 379     assert(input);
 380     assert(r->impl_data);
 381     u = r->impl_data;
 382
 383     /* Convert the data into the correct sample type and place the result in buf4 */
 384
 385     if (!u->from_float32ne_func)
 386         return input;
 387
 388     n_samples = n_frames * r->o_ss.channels;
 389
 390     if (u->buf4_samples < n_samples)
 391         u->buf4 = pa_xrealloc(u->buf4, sizeof(float) * (u->buf4_samples = n_samples));
 392
 393     u->from_float32ne_func(n_samples, input, u->buf4);
 394
 395     return u->buf4;
 396 }
 397
 398 static void libsamplerate_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) {
 399     struct impl_libsamplerate *u;
 400     float *buf;
 401     void *input, *output;
 402     unsigned n_frames;
 403
 404     assert(r);
 405     assert(in);
 406     assert(out);
 407     assert(in->length);
 408     assert(in->memblock);
 409     assert(in->length % r->i_fz == 0);
 410     assert(r->impl_data);
 411
 412     u = r->impl_data;
 413
 414     input = ((uint8_t*) in->memblock->data + in->index);
 415     n_frames = in->length / r->i_fz;
 416     assert(n_frames > 0);
 417
 418     buf = convert_to_float(r, input, n_frames);
 419     buf = remap_channels(r, buf, n_frames);
 420     buf = resample(r, buf, &n_frames);
 421
 422     if (n_frames) {
 423         output = convert_from_float(r, buf, n_frames);
 424
 425         if (output == input) {
 426             /* Mm, no adjustment has been necessary, so let's return the original block */
 427             out->memblock = pa_memblock_ref(in->memblock);
 428             out->index = in->index;
 429             out->length = in->length;
 430         } else {
 431             float **p = NULL;
 432
 433             out->length = n_frames * r->o_fz;
 434             out->index = 0;
 435
 436             if (output == u->buf1) {
 437                 p = &u->buf1;
 438                 u->buf1_samples = 0;
 439             } else if (output == u->buf2) {
 440                 p = &u->buf2;
 441                 u->buf2_samples = 0;
 442             } else if (output == u->buf3) {
 443                 p = &u->buf3;
 444                 u->buf3_samples = 0;
 445             } else if (output == u->buf4) {
 446                 p = &u->buf4;
 447                 u->buf4_samples = 0;
 448             }
 449
 450             assert(p);
 451
 452             /* Take the existing buffer and make it a memblock */
 453             out->memblock = pa_memblock_new_dynamic(*p, out->length, r->memblock_stat);
 454             *p = NULL;
 455         }
 456     } else {
 457         out->memblock = NULL;
 458         out->index = out->length = 0;
 459     }
 460 }
 461
 462 static void libsamplerate_update_input_rate(pa_resampler *r, uint32_t rate) {
 463     struct impl_libsamplerate *u;
 464
 465     assert(r);
 466     assert(rate > 0);
 467     assert(r->impl_data);
 468     u = r->impl_data;
 469
 470     if (!u->src_state) {
 471         int err;
 472         u->src_state = src_new(r->resample_method, r->o_ss.channels, &err);
 473         assert(u->src_state);
 474     } else {
 475         int ret = src_set_ratio(u->src_state, (double) r->o_ss.rate / rate);
 476         assert(ret == 0);
 477     }
 478 }
 479
 480 static int libsamplerate_init(pa_resampler *r) {
 481     struct impl_libsamplerate *u = NULL;
 482     int err;
 483
 484     r->impl_data = u = pa_xnew(struct impl_libsamplerate, 1);
 485
 486     u->buf1 = u->buf2 = u->buf3 = u->buf4 = NULL;
 487     u->buf1_samples = u->buf2_samples = u->buf3_samples = u->buf4_samples = 0;
 488
 489     if (r->i_ss.format == PA_SAMPLE_FLOAT32NE)
 490         u->to_float32ne_func = NULL;
 491     else if (!(u->to_float32ne_func = pa_get_convert_to_float32ne_function(r->i_ss.format)))
 492         goto fail;
 493
 494     if (r->o_ss.format == PA_SAMPLE_FLOAT32NE)
 495         u->from_float32ne_func = NULL;
 496     else if (!(u->from_float32ne_func = pa_get_convert_from_float32ne_function(r->o_ss.format)))
 497         goto fail;
 498
 499     if (r->o_ss.rate == r->i_ss.rate)
 500         u->src_state = NULL;
 501     else if (!(u->src_state = src_new(r->resample_method, r->o_ss.channels, &err)))
 502         goto fail;
 503
 504     r->impl_free = libsamplerate_free;
 505     r->impl_update_input_rate = libsamplerate_update_input_rate;
 506     r->impl_run = libsamplerate_run;
 507
 508     calc_map_table(r);
 509
 510     return 0;
 511
 512 fail:
 513     pa_xfree(u);
 514     return -1;
 515 }
 516
 517 /* Trivial implementation */
 518
 519 static void trivial_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out) {
 520     size_t fz;
 521     unsigned  n_frames;
 522     struct impl_trivial *u;
 523
 524     assert(r);
 525     assert(in);
 526     assert(out);
 527     assert(r->impl_data);
 528
 529     u = r->impl_data;
 530
 531     fz = r->i_fz;
 532     assert(fz == r->o_fz);
 533
 534     n_frames = in->length/fz;
 535
 536     if (r->i_ss.rate == r->o_ss.rate) {
 537
 538         /* In case there's no diefference in sample types, do nothing */
 539         *out = *in;
 540         pa_memblock_ref(out->memblock);
 541
 542         u->o_counter += n_frames;
 543     } else {
 544         /* Do real resampling */
 545         size_t l;
 546         unsigned o_index;
 547
 548         /* The length of the new memory block rounded up */
 549         l = ((((n_frames+1) * r->o_ss.rate) / r->i_ss.rate) + 1) * fz;
 550
 551         out->index = 0;
 552         out->memblock = pa_memblock_new(l, r->memblock_stat);
 553
 554         for (o_index = 0;; o_index++, u->o_counter++) {
 555             unsigned j;
 556
 557             j = (u->o_counter * r->i_ss.rate / r->o_ss.rate);
 558             j = j > u->i_counter ? j - u->i_counter : 0;
 559
 560             if (j >= n_frames)
 561                 break;
 562
 563             assert(o_index*fz < out->memblock->length);
 564
 565             memcpy((uint8_t*) out->memblock->data + fz*o_index,
 566                    (uint8_t*) in->memblock->data + in->index + fz*j, fz);
 567
 568         }
 569
 570         out->length = o_index*fz;
 571     }
 572
 573     u->i_counter += n_frames;
 574
 575     /* Normalize counters */
 576     while (u->i_counter >= r->i_ss.rate) {
 577         u->i_counter -= r->i_ss.rate;
 578         assert(u->o_counter >= r->o_ss.rate);
 579         u->o_counter -= r->o_ss.rate;
 580     }
 581 }
 582
 583 static void trivial_free(pa_resampler *r) {
 584     assert(r);
 585
 586     pa_xfree(r->impl_data);
 587 }
 588
 589 static void trivial_update_input_rate(pa_resampler *r, uint32_t rate) {
 590     struct impl_trivial *u;
 591
 592     assert(r);
 593     assert(rate > 0);
 594     assert(r->impl_data);
 595
 596     u = r->impl_data;
 597     u->i_counter = 0;
 598     u->o_counter = 0;
 599 }
 600
 601 static int trivial_init(pa_resampler*r) {
 602     struct impl_trivial *u;
 603
 604     assert(r);
 605     assert(r->i_ss.format == r->o_ss.format);
 606     assert(r->i_ss.channels == r->o_ss.channels);
 607
 608     r->impl_data = u = pa_xnew(struct impl_trivial, 1);
 609     u->o_counter = u->i_counter = 0;
 610
 611     r->impl_run = trivial_run;
 612     r->impl_free = trivial_free;
 613     r->impl_update_input_rate = trivial_update_input_rate;
 614
 615     return 0;
 616 }
 617
 618