code.delx.au - pulseaudio/blob - polyp/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
  31 #include "resampler.h"
  32 #include "sconv.h"
  33 #include "xmalloc.h"
  34 #include "log.h"
  35
  36 struct pa_resampler {
  37     struct pa_sample_spec i_ss, o_ss;
  38     struct pa_channel_map i_cm, o_cm;
  39     size_t i_fz, o_fz;
  40     struct pa_memblock_stat *memblock_stat;
  41     void *impl_data;
  42     int channels;
  43     pa_resample_method_t resample_method;
  44
  45     void (*impl_free)(struct pa_resampler *r);
  46     void (*impl_set_input_rate)(struct pa_resampler *r, uint32_t rate);
  47     void (*impl_run)(struct pa_resampler *r, const struct pa_memchunk *in, struct pa_memchunk *out);
  48 };
  49
  50 struct impl_libsamplerate {
  51     float* i_buf, *o_buf;
  52     unsigned i_alloc, o_alloc;
  53     pa_convert_to_float32ne_func_t to_float32ne_func;
  54     pa_convert_from_float32ne_func_t from_float32ne_func;
  55     SRC_STATE *src_state;
  56 };
  57
  58 struct impl_trivial {
  59     unsigned o_counter;
  60     unsigned i_counter;
  61 };
  62
  63 static int libsamplerate_init(struct pa_resampler*r);
  64 static int trivial_init(struct pa_resampler*r);
  65
  66 struct pa_resampler* pa_resampler_new(
  67     const struct pa_sample_spec *a,
  68     const struct pa_channel_map *am,
  69     const struct pa_sample_spec *b,
  70     const struct pa_channel_map *bm,
  71     struct pa_memblock_stat *s,
  72     pa_resample_method_t resample_method) {
  73
  74     struct pa_resampler *r = NULL;
  75     assert(a && b && pa_sample_spec_valid(a) && pa_sample_spec_valid(b) && resample_method != PA_RESAMPLER_INVALID);
  76
  77     if (a->channels != b->channels && a->channels != 1 && b->channels != 1)
  78         goto fail;
  79
  80     r = pa_xmalloc(sizeof(struct pa_resampler));
  81     r->impl_data = NULL;
  82     r->memblock_stat = s;
  83     r->resample_method = resample_method;
  84
  85     r->impl_free = NULL;
  86     r->impl_set_input_rate = NULL;
  87     r->impl_run = NULL;
  88
  89     /* Fill sample specs */
  90     r->i_ss = *a;
  91     r->o_ss = *b;
  92
  93     if (am)
  94         r->i_cm = *am;
  95     else
  96         pa_channel_map_init_auto(&r->i_cm, r->i_ss.channels);
  97
  98     if (bm)
  99         r->o_cm = *bm;
 100     else
 101         pa_channel_map_init_auto(&r->o_cm, r->o_ss.channels);
 102
 103
 104     r->i_fz = pa_frame_size(a);
 105     r->o_fz = pa_frame_size(b);
 106
 107     r->channels = a->channels;
 108     if (b->channels < r->channels)
 109         r->channels = b->channels;
 110
 111     /* Choose implementation */
 112     if (a->channels != b->channels || a->format != b->format || resample_method != PA_RESAMPLER_TRIVIAL || !pa_channel_map_equal(&r->i_cm, &r->o_cm)) {
 113         /* Use the libsamplerate based resampler for the complicated cases */
 114         if (resample_method == PA_RESAMPLER_TRIVIAL)
 115             r->resample_method = PA_RESAMPLER_SRC_ZERO_ORDER_HOLD;
 116
 117         if (libsamplerate_init(r) < 0)
 118             goto fail;
 119
 120     } else {
 121         /* Use our own simple non-fp resampler for the trivial cases and when the user selects it */
 122         if (trivial_init(r) < 0)
 123             goto fail;
 124     }
 125
 126     return r;
 127
 128 fail:
 129     if (r)
 130         pa_xfree(r);
 131
 132     return NULL;
 133 }
 134
 135 void pa_resampler_free(struct pa_resampler *r) {
 136     assert(r);
 137
 138     if (r->impl_free)
 139         r->impl_free(r);
 140
 141     pa_xfree(r);
 142 }
 143
 144 void pa_resampler_set_input_rate(struct pa_resampler *r, uint32_t rate) {
 145     assert(r && rate);
 146
 147     r->i_ss.rate = rate;
 148     if (r->impl_set_input_rate)
 149         r->impl_set_input_rate(r, rate);
 150 }
 151
 152 void pa_resampler_run(struct pa_resampler *r, const struct pa_memchunk *in, struct pa_memchunk *out) {
 153     assert(r && in && out && r->impl_run);
 154
 155     r->impl_run(r, in, out);
 156 }
 157
 158 size_t pa_resampler_request(struct pa_resampler *r, size_t out_length) {
 159     assert(r && (out_length % r->o_fz) == 0);
 160     return (((out_length / r->o_fz)*r->i_ss.rate)/r->o_ss.rate) * r->i_fz;
 161 }
 162
 163 pa_resample_method_t pa_resampler_get_method(struct pa_resampler *r) {
 164     assert(r);
 165     return r->resample_method;
 166 }
 167
 168 /*** libsamplerate based implementation ***/
 169
 170 static void libsamplerate_free(struct pa_resampler *r) {
 171     struct impl_libsamplerate *i;
 172     assert(r && r->impl_data);
 173     i = r->impl_data;
 174
 175     if (i->src_state)
 176         src_delete(i->src_state);
 177
 178     pa_xfree(i->i_buf);
 179     pa_xfree(i->o_buf);
 180     pa_xfree(i);
 181 }
 182
 183 static void calc_map_table(struct pa_resampler *r) {
 184     struct impl_libsamplerate *u;
 185     unsigned oc;
 186     assert(r);
 187     assert(r->impl_data);
 188
 189     u = r->impl_data;
 190
 191     if ((u->map_required = (!pa_channel_map_equal(&r->i_cm, r->o_cm) || r->i_ss.channels != r->o_ss.channels))) {
 192
 193         memset(u->map_table, -1, sizeof(u->map_table));
 194
 195         for (oc = 0; oc < r->o_iss.channels; oc++) {
 196             unsigned i = 0, ic;
 197
 198             for (ic = 0; ic < r->i_ss.channels; ic++) {
 199                 pa_channel_position_t a, b;
 200
 201                 a = r->i_cm.map[ic];
 202                 b = r->o_cm.map[oc];
 203
 204                 if (a == b ||
 205                     (a == PA_CHANNEL_POSITION_MONO && b == PA_CHANNEL_POSITION_LEFT) ||
 206                     (a == PA_CHANNEL_POSITION_MONO && b == PA_CHANNEL_POSITION_RIGHT) ||
 207                     (a == PA_CHANNEL_POSITION_LEFT && b == PA_CHANNEL_POSITION_MONO) ||
 208                     (a == PA_CHANNEL_POSITION_RIGHT && b == PA_CHANNEL_POSITION_MONO))
 209
 210                     u->map_table[oc][i++] = ic;
 211             }
 212         }
 213     }
 214 }
 215
 216
 217 static float *remap_to_float(struct pa_resampler *r, const struct pa_memchunk *in) {
 218     unsigned nsamples;
 219     struct impl_libsamplerate *u;
 220     assert(r);
 221     assert(r->impl_data);
 222
 223     u = r->impl_data;
 224
 225     nsamples = in->length / u->i_fz;
 226
 227     if () {
 228
 229         if (u->i_buf_samples < nsamples)
 230             u->i_buf = pa_xrealloc(i->i_buf, sizeof(float) * (i->i_buf_samples = nsamples));
 231
 232         i->to_float32ne_func(ff_ins, (uint8_t*) in->memblock->data+in->index, i_nchannels, i->i_buf);
 233
 234     }
 235
 236
 237 }
 238
 239
 240 static void libsamplerate_run(struct pa_resampler *r, const struct pa_memchunk *in, struct pa_memchunk *out) {
 241
 242
 243
 244 }
 245
 246
 247 static void libsamplerate_run(struct pa_resampler *r, const struct pa_memchunk *in, struct pa_memchunk *out) {
 248     unsigned i_nchannels, o_nchannels, ins, ons, eff_ins, eff_ons;
 249     float *cbuf;
 250     struct impl_libsamplerate *i;
 251     assert(r && in && out && in->length && in->memblock && (in->length % r->i_fz) == 0 && r->impl_data);
 252     i = r->impl_data;
 253
 254     /* How many input samples? */
 255     ins = in->length/r->i_fz;
 256
 257
 258     /*     pa_log("%u / %u = %u\n", in->length, r->i_fz, ins); */
 259
 260     /* How much space for output samples? */
 261     if (i->src_state)
 262         ons = (ins*r->o_ss.rate/r->i_ss.rate)+1024;
 263     else
 264         ons = ins;
 265
 266     /* How many channels? */
 267     if (r->i_ss.channels == r->o_ss.channels) {
 268         i_nchannels = o_nchannels = 1;
 269         eff_ins = ins*r->i_ss.channels; /* effective samples */
 270         eff_ons = ons*r->o_ss.channels;
 271     } else {
 272         i_nchannels = r->i_ss.channels;
 273         o_nchannels = r->o_ss.channels;
 274         eff_ins = ins;
 275         eff_ons = ons;
 276     }
 277
 278 /*     pa_log("eff_ins = %u \n", eff_ins); */
 279
 280
 281     out->memblock = pa_memblock_new(out->length = (ons*r->o_fz), r->memblock_stat);
 282     out->index = 0;
 283     assert(out->memblock);
 284
 285     if (i->i_alloc < eff_ins)
 286         i->i_buf = pa_xrealloc(i->i_buf, sizeof(float) * (i->i_alloc = eff_ins));
 287     assert(i->i_buf);
 288
 289 /*     pa_log("eff_ins = %u \n", eff_ins); */
 290
 291     i->to_float32ne_func(eff_ins, (uint8_t*) in->memblock->data+in->index, i_nchannels, i->i_buf);
 292
 293     if (i->src_state) {
 294         int ret;
 295         SRC_DATA data;
 296
 297         if (i->o_alloc < eff_ons)
 298             i->o_buf = pa_xrealloc(i->o_buf, sizeof(float) * (i->o_alloc = eff_ons));
 299         assert(i->o_buf);
 300
 301         data.data_in = i->i_buf;
 302         data.input_frames = ins;
 303
 304         data.data_out = i->o_buf;
 305         data.output_frames = ons;
 306
 307         data.src_ratio = (double) r->o_ss.rate / r->i_ss.rate;
 308         data.end_of_input = 0;
 309
 310         ret = src_process(i->src_state, &data);
 311         assert(ret == 0);
 312         assert((unsigned) data.input_frames_used == ins);
 313
 314         cbuf = i->o_buf;
 315         ons = data.output_frames_gen;
 316
 317         if (r->i_ss.channels == r->o_ss.channels)
 318             eff_ons = ons*r->o_ss.channels;
 319         else
 320             eff_ons = ons;
 321     } else
 322         cbuf = i->i_buf;
 323
 324     if (eff_ons)
 325         i->from_float32ne_func(eff_ons, cbuf, (uint8_t*)out->memblock->data+out->index, o_nchannels);
 326     out->length = ons*r->o_fz;
 327
 328     if (!out->length) {
 329         pa_memblock_unref(out->memblock);
 330         out->memblock = NULL;
 331     }
 332 }
 333
 334 static void libsamplerate_set_input_rate(struct pa_resampler *r, uint32_t rate) {
 335     int ret;
 336     struct impl_libsamplerate *i;
 337     assert(r && rate > 0 && r->impl_data);
 338     i = r->impl_data;
 339
 340     ret = src_set_ratio(i->src_state, (double) r->o_ss.rate / r->i_ss.rate);
 341     assert(ret == 0);
 342 }
 343
 344 static int libsamplerate_init(struct pa_resampler *r) {
 345     struct impl_libsamplerate *i = NULL;
 346     int err;
 347
 348     r->impl_data = i = pa_xmalloc(sizeof(struct impl_libsamplerate));
 349
 350     i->to_float32ne_func = pa_get_convert_to_float32ne_function(r->i_ss.format);
 351     i->from_float32ne_func = pa_get_convert_from_float32ne_function(r->o_ss.format);
 352
 353     if (!i->to_float32ne_func || !i->from_float32ne_func)
 354         goto fail;
 355
 356     if (!(i->src_state = src_new(r->resample_method, r->channels, &err)) || !i->src_state)
 357         goto fail;
 358
 359     i->i_buf = i->o_buf = NULL;
 360     i->i_alloc = i->o_alloc = 0;
 361
 362     r->impl_free = libsamplerate_free;
 363     r->impl_set_input_rate = libsamplerate_set_input_rate;
 364     r->impl_run = libsamplerate_run;
 365
 366     return 0;
 367
 368 fail:
 369     pa_xfree(i);
 370     return -1;
 371 }
 372
 373 /* Trivial implementation */
 374
 375 static void trivial_run(struct pa_resampler *r, const struct pa_memchunk *in, struct pa_memchunk *out) {
 376     size_t fz;
 377     unsigned  nframes;
 378     struct impl_trivial *i;
 379     assert(r && in && out && r->impl_data);
 380     i = r->impl_data;
 381
 382     fz = r->i_fz;
 383     assert(fz == r->o_fz);
 384
 385     nframes = in->length/fz;
 386
 387     if (r->i_ss.rate == r->o_ss.rate) {
 388
 389         /* In case there's no diefference in sample types, do nothing */
 390         *out = *in;
 391         pa_memblock_ref(out->memblock);
 392
 393         i->o_counter += nframes;
 394     } else {
 395         /* Do real resampling */
 396         size_t l;
 397         unsigned o_index;
 398
 399         /* The length of the new memory block rounded up */
 400         l = ((((nframes+1) * r->o_ss.rate) / r->i_ss.rate) + 1) * fz;
 401
 402         out->index = 0;
 403         out->memblock = pa_memblock_new(l, r->memblock_stat);
 404
 405         for (o_index = 0;; o_index++, i->o_counter++) {
 406             unsigned j;
 407
 408             j = (i->o_counter * r->i_ss.rate / r->o_ss.rate);
 409             j = j > i->i_counter ? j - i->i_counter : 0;
 410
 411             if (j >= nframes)
 412                 break;
 413
 414             assert(o_index*fz < out->memblock->length);
 415
 416             memcpy((uint8_t*) out->memblock->data + fz*o_index,
 417                    (uint8_t*) in->memblock->data + in->index + fz*j, fz);
 418
 419         }
 420
 421         out->length = o_index*fz;
 422     }
 423
 424     i->i_counter += nframes;
 425
 426     /* Normalize counters */
 427     while (i->i_counter >= r->i_ss.rate) {
 428         i->i_counter -= r->i_ss.rate;
 429         assert(i->o_counter >= r->o_ss.rate);
 430         i->o_counter -= r->o_ss.rate;
 431     }
 432 }
 433
 434 static void trivial_free(struct pa_resampler *r) {
 435     assert(r);
 436     pa_xfree(r->impl_data);
 437 }
 438
 439 static void trivial_set_input_rate(struct pa_resampler *r, uint32_t rate) {
 440     struct impl_trivial *i;
 441     assert(r && rate > 0 && r->impl_data);
 442     i = r->impl_data;
 443
 444     i->i_counter = 0;
 445     i->o_counter = 0;
 446 }
 447
 448 static int trivial_init(struct pa_resampler*r) {
 449     struct impl_trivial *i;
 450     assert(r && r->i_ss.format == r->o_ss.format && r->i_ss.channels == r->o_ss.channels);
 451
 452     r->impl_data = i = pa_xmalloc(sizeof(struct impl_trivial));
 453     i->o_counter = i->i_counter = 0;
 454
 455     r->impl_run = trivial_run;
 456     r->impl_free = trivial_free;
 457     r->impl_set_input_rate = trivial_set_input_rate;
 458
 459     return 0;
 460 }
 461
 462 const char *pa_resample_method_to_string(pa_resample_method_t m) {
 463     static const char * const resample_methods[] = {
 464         "src-sinc-best-quality",
 465         "src-sinc-medium-quality",
 466         "src-sinc-fastest",
 467         "src-zero-order-hold",
 468         "src-linear",
 469         "trivial"
 470     };
 471
 472     if (m < 0 || m >= PA_RESAMPLER_MAX)
 473         return NULL;
 474
 475     return resample_methods[m];
 476 }
 477
 478 pa_resample_method_t pa_parse_resample_method(const char *string) {
 479     assert(string);
 480
 481     if (!strcmp(string, "src-sinc-best-quality"))
 482         return PA_RESAMPLER_SRC_SINC_BEST_QUALITY;
 483     else if (!strcmp(string, "src-sinc-medium-quality"))
 484         return PA_RESAMPLER_SRC_SINC_MEDIUM_QUALITY;
 485     else if (!strcmp(string, "src-sinc-fastest"))
 486         return PA_RESAMPLER_SRC_SINC_FASTEST;
 487     else if (!strcmp(string, "src-zero-order-hold"))
 488         return PA_RESAMPLER_SRC_ZERO_ORDER_HOLD;
 489     else if (!strcmp(string, "src-linear"))
 490         return PA_RESAMPLER_SRC_LINEAR;
 491     else if (!strcmp(string, "trivial"))
 492         return PA_RESAMPLER_TRIVIAL;
 493     else
 494         return PA_RESAMPLER_INVALID;
 495 }
 496