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