gst-libs/gst/audio/audio-resampler.c

   1 /* GStreamer
   2  * Copyright (C) <2015> Wim Taymans <wim.taymans@gmail.com>
   3  *
   4  * This library is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU Library General Public
   6  * License as published by the Free Software Foundation; either
   7  * version 2 of the License, or (at your option) any later version.
   8  *
   9  * This library is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * Library General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU Library General Public
  15  * License along with this library; if not, write to the
  16  * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
  17  * Boston, MA 02110-1301, USA.
  18  */
  19
  20 #ifdef HAVE_CONFIG_H
  21 #  include "config.h"
  22 #endif
  23
  24 #include <string.h>
  25 #include <stdio.h>
  26 #include <math.h>
  27
  28 #ifdef HAVE_ORC
  29 #include <orc/orc.h>
  30 #endif
  31
  32 #include "audio-resampler.h"
  33
  34 typedef struct _Tap
  35 {
  36   gpointer taps;
  37 } Tap;
  38
  39 typedef void (*MakeTapsFunc) (GstAudioResampler * resampler, Tap * t, gint j);
  40 typedef void (*ResampleFunc) (GstAudioResampler * resampler, gpointer in[],
  41     gsize in_len, gpointer out[], gsize out_len, gsize * consumed);
  42 typedef void (*DeinterleaveFunc) (GstAudioResampler * resampler,
  43     gpointer * sbuf, gpointer in[], gsize in_frames);
  44
  45 #define MEM_ALIGN(m,a) ((gint8 *)((guintptr)((gint8 *)(m) + ((a)-1)) & ~((a)-1)))
  46 #define ALIGN 16
  47 #define TAPS_OVERREAD 16
  48
  49 struct _GstAudioResampler
  50 {
  51   GstAudioResamplerMethod method;
  52   GstAudioResamplerFlags flags;
  53   GstAudioFormat format;
  54   GstStructure *options;
  55   gint channels;
  56   gint in_rate;
  57   gint out_rate;
  58   gint bps;
  59   gint ostride;
  60
  61   gdouble cutoff;
  62   gdouble kaiser_beta;
  63   /* for cubic */
  64   gdouble b, c;
  65
  66   GstAudioResamplerFilterMode filter_mode;
  67   guint filter_threshold;
  68   GstAudioResamplerFilterInterpolation filter_interpolation;
  69   gint oversample;
  70
  71   guint n_taps;
  72   Tap *taps;
  73   gpointer coeff;
  74   gpointer coeffmem;
  75   guint alloc_taps;
  76   guint alloc_phases;
  77   gsize cstride;
  78   gpointer tmpcoeff;
  79
  80   DeinterleaveFunc deinterleave;
  81   ResampleFunc resample;
  82
  83   guint blocks;
  84   guint inc;
  85   gint samp_inc;
  86   gint samp_frac;
  87   gint samp_index;
  88   gint samp_phase;
  89   gint skip;
  90
  91   gpointer samples;
  92   gsize samples_len;
  93   gsize samples_avail;
  94   gpointer *sbuf;
  95 };
  96
  97 GST_DEBUG_CATEGORY_STATIC (audio_resampler_debug);
  98 #define GST_CAT_DEFAULT audio_resampler_debug
  99
 100 /**
 101  * SECTION:gstaudioresampler
 102  * @short_description: Utility structure for resampler information
 103  *
 104  * #GstAudioResampler is a structure which holds the information
 105  * required to perform various kinds of resampling filtering.
 106  *
 107  */
 108
 109 static const gint oversample_qualities[] = {
 110   4, 4, 4, 8, 8, 16, 16, 16, 16, 32, 32
 111 };
 112
 113 typedef struct
 114 {
 115   gdouble cutoff;
 116   gdouble downsample_cutoff_factor;
 117   gdouble stopband_attenuation;
 118   gdouble transition_bandwidth;
 119 } KaiserQualityMap;
 120
 121 static const KaiserQualityMap kaiser_qualities[] = {
 122   {0.860, 0.96511, 60, 0.7},    /* 8 taps */
 123   {0.880, 0.96591, 65, 0.29},   /* 16 taps */
 124   {0.910, 0.96923, 70, 0.145},  /* 32 taps */
 125   {0.920, 0.97600, 80, 0.105},  /* 48 taps */
 126   {0.940, 0.97979, 85, 0.087},  /* 64 taps default quality */
 127   {0.940, 0.98085, 95, 0.077},  /* 80 taps */
 128   {0.945, 0.99471, 100, 0.068}, /* 96 taps */
 129   {0.950, 1.0, 105, 0.055},     /* 128 taps */
 130   {0.960, 1.0, 110, 0.045},     /* 160 taps */
 131   {0.968, 1.0, 115, 0.039},     /* 192 taps */
 132   {0.975, 1.0, 120, 0.0305}     /* 256 taps */
 133 };
 134
 135 typedef struct
 136 {
 137   guint n_taps;
 138   gdouble cutoff;
 139 } BlackmanQualityMap;
 140
 141 static const BlackmanQualityMap blackman_qualities[] = {
 142   {8, 0.5,},
 143   {16, 0.6,},
 144   {24, 0.72,},
 145   {32, 0.8,},
 146   {48, 0.85,},                  /* default */
 147   {64, 0.90,},
 148   {80, 0.92,},
 149   {96, 0.933,},
 150   {128, 0.950,},
 151   {148, 0.955,},
 152   {160, 0.960,}
 153 };
 154
 155 #define DEFAULT_QUALITY GST_AUDIO_RESAMPLER_QUALITY_DEFAULT
 156 #define DEFAULT_OPT_CUBIC_B 1.0
 157 #define DEFAULT_OPT_CUBIC_C 0.0
 158 #define DEFAULT_OPT_FILTER_MODE GST_AUDIO_RESAMPLER_FILTER_MODE_AUTO
 159 #define DEFAULT_OPT_FILTER_MODE_THRESHOLD 1048576
 160 #define DEFAULT_OPT_FILTER_INTERPOLATION GST_AUDIO_RESAMPLER_FILTER_INTERPOLATION_LINEAR
 161 #define DEFAULT_OPT_FILTER_OVERSAMPLE 8
 162 #define DEFAULT_OPT_MAX_PHASE_ERROR 0.1
 163
 164 static gdouble
 165 get_opt_double (GstStructure * options, const gchar * name, gdouble def)
 166 {
 167   gdouble res;
 168   if (!options || !gst_structure_get_double (options, name, &res))
 169     res = def;
 170   return res;
 171 }
 172
 173 static gint
 174 get_opt_int (GstStructure * options, const gchar * name, gint def)
 175 {
 176   gint res;
 177   if (!options || !gst_structure_get_int (options, name, &res))
 178     res = def;
 179   return res;
 180 }
 181
 182 static gint
 183 get_opt_enum (GstStructure * options, const gchar * name, GType type, gint def)
 184 {
 185   gint res;
 186   if (!options || !gst_structure_get_enum (options, name, type, &res))
 187     res = def;
 188   return res;
 189 }
 190
 191
 192 #define GET_OPT_CUTOFF(options,def) get_opt_double(options, \
 193     GST_AUDIO_RESAMPLER_OPT_CUTOFF,def)
 194 #define GET_OPT_DOWN_CUTOFF_FACTOR(options,def) get_opt_double(options, \
 195     GST_AUDIO_RESAMPLER_OPT_DOWN_CUTOFF_FACTOR, def)
 196 #define GET_OPT_STOP_ATTENUATION(options,def) get_opt_double(options, \
 197     GST_AUDIO_RESAMPLER_OPT_STOP_ATTENUATION, def)
 198 #define GET_OPT_TRANSITION_BANDWIDTH(options,def) get_opt_double(options, \
 199     GST_AUDIO_RESAMPLER_OPT_TRANSITION_BANDWIDTH, def)
 200 #define GET_OPT_CUBIC_B(options) get_opt_double(options, \
 201     GST_AUDIO_RESAMPLER_OPT_CUBIC_B, DEFAULT_OPT_CUBIC_B)
 202 #define GET_OPT_CUBIC_C(options) get_opt_double(options, \
 203     GST_AUDIO_RESAMPLER_OPT_CUBIC_C, DEFAULT_OPT_CUBIC_C)
 204 #define GET_OPT_N_TAPS(options,def) get_opt_int(options, \
 205     GST_AUDIO_RESAMPLER_OPT_N_TAPS, def)
 206 #define GET_OPT_FILTER_MODE(options) get_opt_enum(options, \
 207     GST_AUDIO_RESAMPLER_OPT_FILTER_MODE, GST_TYPE_AUDIO_RESAMPLER_FILTER_MODE, \
 208     DEFAULT_OPT_FILTER_MODE)
 209 #define GET_OPT_FILTER_MODE_THRESHOLD(options) get_opt_int(options, \
 210     GST_AUDIO_RESAMPLER_OPT_FILTER_MODE_THRESHOLD, DEFAULT_OPT_FILTER_MODE_THRESHOLD)
 211 #define GET_OPT_FILTER_INTERPOLATION(options) get_opt_enum(options, \
 212     GST_AUDIO_RESAMPLER_OPT_FILTER_INTERPOLATION, GST_TYPE_AUDIO_RESAMPLER_FILTER_INTERPOLATION, \
 213     DEFAULT_OPT_FILTER_INTERPOLATION)
 214 #define GET_OPT_FILTER_OVERSAMPLE(options) get_opt_int(options, \
 215     GST_AUDIO_RESAMPLER_OPT_FILTER_OVERSAMPLE, DEFAULT_OPT_FILTER_OVERSAMPLE)
 216 #define GET_OPT_MAX_PHASE_ERROR(options) get_opt_double(options, \
 217     GST_AUDIO_RESAMPLER_OPT_MAX_PHASE_ERROR, DEFAULT_OPT_MAX_PHASE_ERROR)
 218
 219 #include "dbesi0.c"
 220 #define bessel dbesi0
 221
 222 static inline gdouble
 223 get_nearest_tap (gdouble x)
 224 {
 225   gdouble a = fabs (x);
 226
 227   if (a < 0.5)
 228     return 1.0;
 229   else
 230     return 0.0;
 231 }
 232
 233 static inline gdouble
 234 get_linear_tap (gdouble x, gint n_taps)
 235 {
 236   gdouble a;
 237
 238   a = fabs (x) / n_taps;
 239
 240   if (a < 1.0)
 241     return 1.0 - a;
 242   else
 243     return 0.0;
 244 }
 245
 246 static inline gdouble
 247 get_cubic_tap (gdouble x, gint n_taps, gdouble b, gdouble c)
 248 {
 249   gdouble a, a2, a3;
 250
 251   a = fabs (x * 4.0) / n_taps;
 252   a2 = a * a;
 253   a3 = a2 * a;
 254
 255   if (a <= 1.0)
 256     return ((12.0 - 9.0 * b - 6.0 * c) * a3 +
 257         (-18.0 + 12.0 * b + 6.0 * c) * a2 + (6.0 - 2.0 * b)) / 6.0;
 258   else if (a <= 2.0)
 259     return ((-b - 6.0 * c) * a3 +
 260         (6.0 * b + 30.0 * c) * a2 +
 261         (-12.0 * b - 48.0 * c) * a + (8.0 * b + 24.0 * c)) / 6.0;
 262   else
 263     return 0.0;
 264 }
 265
 266 static inline gdouble
 267 get_blackman_nuttall_tap (gdouble x, gint n_taps, gdouble Fc)
 268 {
 269   gdouble s, y, w;
 270
 271   y = G_PI * x;
 272   s = (y == 0.0 ? Fc : sin (y * Fc) / y);
 273
 274   w = 2.0 * y / n_taps + G_PI;
 275   return s * (0.3635819 - 0.4891775 * cos (w) + 0.1365995 * cos (2 * w) -
 276       0.0106411 * cos (3 * w));
 277 }
 278
 279 static inline gdouble
 280 get_kaiser_tap (gdouble x, gint n_taps, gdouble Fc, gdouble beta)
 281 {
 282   gdouble s, y, w;
 283
 284   y = G_PI * x;
 285   s = (y == 0.0 ? Fc : sin (y * Fc) / y);
 286
 287   w = 2.0 * x / n_taps;
 288   return s * bessel (beta * sqrt (MAX (1 - w * w, 0)));
 289 }
 290
 291 #define PRECISION_S16 14
 292 #define PRECISION_S32 30
 293
 294 static inline gdouble
 295 fill_taps (GstAudioResampler * resampler,
 296     gdouble * tmpcoeff, gdouble x, gint n_taps, gint oversample)
 297 {
 298   gdouble weight = 0.0;
 299   gint i;
 300
 301   switch (resampler->method) {
 302     case GST_AUDIO_RESAMPLER_METHOD_NEAREST:
 303       for (i = 0; i < n_taps; i++)
 304         weight += tmpcoeff[i] = get_nearest_tap (x + i / (double) oversample);
 305       break;
 306
 307     case GST_AUDIO_RESAMPLER_METHOD_LINEAR:
 308       for (i = 0; i < n_taps; i++)
 309         weight += tmpcoeff[i] =
 310             get_linear_tap (x + i / (double) oversample, resampler->n_taps);
 311       break;
 312
 313     case GST_AUDIO_RESAMPLER_METHOD_CUBIC:
 314       for (i = 0; i < n_taps; i++)
 315         weight += tmpcoeff[i] =
 316             get_cubic_tap (x + i / (double) oversample, resampler->n_taps,
 317             resampler->b, resampler->c);
 318       break;
 319
 320     case GST_AUDIO_RESAMPLER_METHOD_BLACKMAN_NUTTALL:
 321       for (i = 0; i < n_taps; i++)
 322         weight += tmpcoeff[i] =
 323             get_blackman_nuttall_tap (x + i / (double) oversample,
 324             resampler->n_taps, resampler->cutoff);
 325       break;
 326
 327     case GST_AUDIO_RESAMPLER_METHOD_KAISER:
 328       for (i = 0; i < n_taps; i++)
 329         weight += tmpcoeff[i] =
 330             get_kaiser_tap (x + i / (double) oversample, resampler->n_taps,
 331             resampler->cutoff, resampler->kaiser_beta);
 332       break;
 333
 334     default:
 335       break;
 336   }
 337   return weight;
 338 }
 339
 340 #define MAKE_CONVERT_TAPS_INT_FUNC(type, precision)                     \
 341 static inline void                                                      \
 342 convert_taps_##type (gdouble *tmpcoeff, type *taps,                     \
 343     gdouble weight, gint n_taps)                                        \
 344 {                                                                       \
 345   gdouble multiplier = (1 << precision);                                \
 346   gint i, j;                                                            \
 347   gdouble offset, l_offset, h_offset;                                   \
 348   gboolean exact = FALSE;                                               \
 349   /* Round to integer, but with an adjustable bias that we use to */    \
 350   /* eliminate the DC error. */                                         \
 351   l_offset = 0.0;                                                       \
 352   h_offset = 1.0;                                                       \
 353   offset = 0.5;                                                         \
 354   for (i = 0; i < 32; i++) {                                            \
 355     gint64 sum = 0;                                                     \
 356     for (j = 0; j < n_taps; j++)                                        \
 357       sum += taps[j] = floor (offset + tmpcoeff[j] * multiplier / weight); \
 358     if (sum == (1 << precision)) {                                      \
 359       exact = TRUE;                                                     \
 360       break;                                                            \
 361     }                                                                   \
 362     if (l_offset == h_offset)                                           \
 363       break;                                                            \
 364     if (sum < (1 << precision)) {                                       \
 365       if (offset > l_offset)                                            \
 366         l_offset = offset;                                              \
 367       offset += (h_offset - l_offset) / 2;                              \
 368     } else {                                                            \
 369       if (offset < h_offset)                                            \
 370         h_offset = offset;                                              \
 371       offset -= (h_offset - l_offset) / 2;                              \
 372     }                                                                   \
 373   }                                                                     \
 374   if (!exact)                                                           \
 375     GST_WARNING ("can't find exact taps");                              \
 376 }
 377
 378 #define MAKE_CONVERT_TAPS_FLOAT_FUNC(type)                              \
 379 static inline void                                                      \
 380 convert_taps_##type (gdouble *tmpcoeff, type *taps,                     \
 381     gdouble weight, gint n_taps)                                        \
 382 {                                                                       \
 383   gint i;                                                               \
 384   for (i = 0; i < n_taps; i++)                                          \
 385     taps[i] = tmpcoeff[i] / weight;                                     \
 386 }
 387
 388 MAKE_CONVERT_TAPS_INT_FUNC (gint16, PRECISION_S16);
 389 MAKE_CONVERT_TAPS_INT_FUNC (gint32, PRECISION_S32);
 390 MAKE_CONVERT_TAPS_FLOAT_FUNC (gfloat);
 391 MAKE_CONVERT_TAPS_FLOAT_FUNC (gdouble);
 392
 393 #define GET_TAPS_NONE_FUNC(type)                                                \
 394 static inline gpointer                                                          \
 395 get_taps_##type##_none (GstAudioResampler * resampler,                          \
 396     gint *samp_index, gint *samp_phase, type icoeff[4])                         \
 397 {                                                                               \
 398   Tap *t = &resampler->taps[*samp_phase];                                       \
 399   gpointer res;                                                                 \
 400   gdouble x, weight;                                                            \
 401   gint out_rate = resampler->out_rate;                                          \
 402   gdouble *tmpcoeff = resampler->tmpcoeff;                                      \
 403   gint n_taps = resampler->n_taps;                                              \
 404                                                                                 \
 405   if (G_LIKELY (t->taps)) {                                                     \
 406     res = t->taps;                                                              \
 407   } else {                                                                      \
 408     res = (gint8 *) resampler->coeff + *samp_phase * resampler->cstride;        \
 409                                                                                 \
 410     x = 1.0 - n_taps / 2 - (double) *samp_phase / out_rate;                     \
 411     weight = fill_taps (resampler, tmpcoeff, x, n_taps, 1);                     \
 412     convert_taps_##type (tmpcoeff, res, weight, n_taps);                        \
 413                                                                                 \
 414     t->taps = res;                                                              \
 415   }                                                                             \
 416   *samp_index += resampler->samp_inc;                                           \
 417   *samp_phase += resampler->samp_frac;                                          \
 418   if (*samp_phase >= out_rate) {                                                \
 419     *samp_phase -= out_rate;                                                    \
 420     (*samp_index)++;                                                            \
 421   }                                                                             \
 422   return res;                                                                   \
 423 }
 424
 425 GET_TAPS_NONE_FUNC (gint16);
 426 GET_TAPS_NONE_FUNC (gint32);
 427 GET_TAPS_NONE_FUNC (gfloat);
 428 GET_TAPS_NONE_FUNC (gdouble);
 429
 430 #define MAKE_COEFF_LINEAR_FLOAT_FUNC(type)                              \
 431 static inline void                                                      \
 432 make_coeff_##type##_linear (gint frac, gint out_rate, type *icoeff)     \
 433 {                                                                       \
 434   type x = (type)frac / out_rate;                                       \
 435   icoeff[0] = icoeff[2] = 1.0 - x;                                      \
 436   icoeff[1] = icoeff[3] = x;                                            \
 437 }
 438 #define MAKE_COEFF_LINEAR_INT_FUNC(type,type2,prec)                     \
 439 static inline void                                                      \
 440 make_coeff_##type##_linear (gint frac, gint out_rate, type *icoeff)     \
 441 {                                                                       \
 442   type x = ((type2)frac << prec) / out_rate;                            \
 443   icoeff[0] = icoeff[2] = (1 << prec) - x;                              \
 444   icoeff[1] = icoeff[3] = x;                                            \
 445 }
 446
 447 MAKE_COEFF_LINEAR_INT_FUNC (gint16, gint32, PRECISION_S16);
 448 MAKE_COEFF_LINEAR_INT_FUNC (gint32, gint64, PRECISION_S32);
 449 MAKE_COEFF_LINEAR_FLOAT_FUNC (gfloat);
 450 MAKE_COEFF_LINEAR_FLOAT_FUNC (gdouble);
 451
 452 #define GET_TAPS_LINEAR_FUNC(type)                              \
 453 static inline gpointer                                          \
 454 get_taps_##type##_linear (GstAudioResampler * resampler,        \
 455     gint *samp_index, gint *samp_phase, type icoeff[4])         \
 456 {                                                               \
 457   gpointer res;                                                 \
 458   gint out_rate = resampler->out_rate;                          \
 459   gint offset, frac, pos;                                       \
 460                                                                 \
 461   pos = ((out_rate - 1) - *samp_phase) * resampler->oversample; \
 462   offset = pos / out_rate;                                      \
 463   frac = pos % out_rate;                                        \
 464                                                                 \
 465   res = (type *)resampler->coeff + offset;                      \
 466   make_coeff_##type##_linear (frac, out_rate, icoeff);          \
 467                                                                 \
 468   *samp_index += resampler->samp_inc;                           \
 469   *samp_phase += resampler->samp_frac;                          \
 470   if (*samp_phase >= out_rate) {                                \
 471     *samp_phase -= out_rate;                                    \
 472     (*samp_index)++;                                            \
 473   }                                                             \
 474   return res;                                                   \
 475 }
 476
 477 GET_TAPS_LINEAR_FUNC (gint16);
 478 GET_TAPS_LINEAR_FUNC (gint32);
 479 GET_TAPS_LINEAR_FUNC (gfloat);
 480 GET_TAPS_LINEAR_FUNC (gdouble);
 481
 482 #define INNER_PRODUCT_INT_NONE_FUNC(type,type2,prec,limit)      \
 483 static inline void                                              \
 484 inner_product_##type##_none_1_c (type * o, const type * a,      \
 485     const type * b, gint len, const type *ic, gint oversample)  \
 486 {                                                               \
 487   gint i;                                                       \
 488   type2 res = 0;                                                \
 489                                                                 \
 490   for (i = 0; i < len; i++)                                     \
 491     res += (type2) a[i] * (type2) b[i];                         \
 492                                                                 \
 493   res = (res + (1 << ((prec) - 1))) >> (prec);                  \
 494   *o = CLAMP (res, -(limit), (limit) - 1);                      \
 495 }
 496
 497 INNER_PRODUCT_INT_NONE_FUNC (gint16, gint32, PRECISION_S16, 1L << 15);
 498 INNER_PRODUCT_INT_NONE_FUNC (gint32, gint64, PRECISION_S32, 1L << 31);
 499
 500 #define INNER_PRODUCT_INT_LINEAR_FUNC(type,type2,prec,limit)    \
 501 static inline void                                              \
 502 inner_product_##type##_linear_1_c (type * o, const type * a,    \
 503     const type * b, gint len, const type *ic, gint oversample)  \
 504 {                                                               \
 505   gint i;                                                       \
 506   type2 res, res1 = 0, res2 = 0;                                \
 507                                                                 \
 508   for (i = 0; i < len; i++) {                                   \
 509     res1 += (type2) a[i] * (type2) b[i * oversample];           \
 510     res2 += (type2) a[i] * (type2) b[i * oversample + 1];       \
 511   }                                                             \
 512   res = (res1 >> (prec)) * ic[0] + (res2 >> (prec)) * ic[1];    \
 513   res = (res + (1 << ((prec) - 1))) >> (prec);                  \
 514   *o = CLAMP (res, -(limit), (limit) - 1);                      \
 515 }
 516
 517 INNER_PRODUCT_INT_LINEAR_FUNC (gint16, gint32, PRECISION_S16, 1L << 15);
 518 INNER_PRODUCT_INT_LINEAR_FUNC (gint32, gint64, PRECISION_S32, 1L << 31);
 519
 520 #define INNER_PRODUCT_FLOAT_NONE_FUNC(type)                     \
 521 static inline void                                              \
 522 inner_product_##type##_none_1_c (type * o, const type * a,      \
 523     const type * b, gint len, const type *ic, gint oversample)  \
 524 {                                                               \
 525   gint i;                                                       \
 526   type res = 0.0;                                               \
 527                                                                 \
 528   for (i = 0; i < len; i++)                                     \
 529     res += a[i] * b[i];                                         \
 530                                                                 \
 531   *o = res;                                                     \
 532 }
 533
 534 INNER_PRODUCT_FLOAT_NONE_FUNC (gfloat);
 535 INNER_PRODUCT_FLOAT_NONE_FUNC (gdouble);
 536
 537 #define INNER_PRODUCT_FLOAT_LINEAR_FUNC(type)                   \
 538 static inline void                                              \
 539 inner_product_##type##_linear_1_c (type * o, const type * a,    \
 540     const type * b, gint len, const type *ic, gint oversample)  \
 541 {                                                               \
 542   gint i;                                                       \
 543   type res1 = 0.0, res2 = 0.0;                                  \
 544                                                                 \
 545   for (i = 0; i < len; i++) {                                   \
 546     res1 += a[i] * b[i * oversample];                           \
 547     res2 += a[i] * b[i * oversample + 1];                       \
 548   }                                                             \
 549   *o = res1 * ic[0] + res2 * ic[1];                             \
 550 }
 551 INNER_PRODUCT_FLOAT_LINEAR_FUNC (gfloat);
 552 INNER_PRODUCT_FLOAT_LINEAR_FUNC (gdouble);
 553
 554 #define MAKE_RESAMPLE_FUNC(type,inter,channels,arch)                            \
 555 static void                                                                     \
 556 resample_ ##type## _ ##inter## _ ##channels## _ ##arch (GstAudioResampler * resampler,      \
 557     gpointer in[], gsize in_len,  gpointer out[], gsize out_len,                \
 558     gsize * consumed)                                                           \
 559 {                                                                               \
 560   gint c, di = 0;                                                               \
 561   gint n_taps = resampler->n_taps;                                              \
 562   gint blocks = resampler->blocks;                                              \
 563   gint ostride = resampler->ostride;                                            \
 564   gint oversample = resampler->oversample;                                      \
 565   gint samp_index = 0;                                                          \
 566   gint samp_phase = 0;                                                          \
 567                                                                                 \
 568   for (c = 0; c < blocks; c++) {                                                \
 569     type *ip = in[c];                                                           \
 570     type *op = ostride == 1 ? out[c] : (type *)out[0] + c;                      \
 571                                                                                 \
 572     samp_index = resampler->samp_index;                                         \
 573     samp_phase = resampler->samp_phase;                                         \
 574                                                                                 \
 575     for (di = 0; di < out_len; di++) {                                          \
 576       type *ipp, icoeff[4], *taps;                                              \
 577                                                                                 \
 578       ipp = &ip[samp_index * channels];                                         \
 579                                                                                 \
 580       taps = get_taps_ ##type##_##inter (resampler, &samp_index, &samp_phase, icoeff);                   \
 581                                                                                 \
 582       inner_product_ ##type##_##inter##_##channels##_##arch (op, ipp, taps, n_taps, icoeff, oversample);     \
 583       op += ostride;                                                            \
 584     }                                                                           \
 585     memmove (ip, &ip[samp_index * channels],                                    \
 586         (in_len - samp_index) * sizeof(type) * channels);                       \
 587   }                                                                             \
 588   *consumed = samp_index - resampler->samp_index;                               \
 589                                                                                 \
 590   resampler->samp_index = 0;                                                    \
 591   resampler->samp_phase = samp_phase;                                           \
 592 }
 593
 594 MAKE_RESAMPLE_FUNC (gint16, none, 1, c);
 595 MAKE_RESAMPLE_FUNC (gint32, none, 1, c);
 596 MAKE_RESAMPLE_FUNC (gfloat, none, 1, c);
 597 MAKE_RESAMPLE_FUNC (gdouble, none, 1, c);
 598
 599 MAKE_RESAMPLE_FUNC (gint16, linear, 1, c);
 600 MAKE_RESAMPLE_FUNC (gint32, linear, 1, c);
 601 MAKE_RESAMPLE_FUNC (gfloat, linear, 1, c);
 602 MAKE_RESAMPLE_FUNC (gdouble, linear, 1, c);
 603
 604 static ResampleFunc resample_funcs[] = {
 605   resample_gint16_none_1_c,
 606   resample_gint32_none_1_c,
 607   resample_gfloat_none_1_c,
 608   resample_gdouble_none_1_c,
 609   NULL,
 610   NULL,
 611   NULL,
 612   NULL,
 613
 614   resample_gint16_linear_1_c,
 615   resample_gint32_linear_1_c,
 616   resample_gfloat_linear_1_c,
 617   resample_gdouble_linear_1_c,
 618   NULL,
 619   NULL,
 620   NULL,
 621   NULL,
 622 };
 623
 624 #define resample_gint16_none_1 resample_funcs[0]
 625 #define resample_gint32_none_1 resample_funcs[1]
 626 #define resample_gfloat_none_1 resample_funcs[2]
 627 #define resample_gdouble_none_1 resample_funcs[3]
 628 #define resample_gint16_none_2 resample_funcs[4]
 629 #define resample_gint32_none_2 resample_funcs[5]
 630 #define resample_gfloat_none_2 resample_funcs[6]
 631 #define resample_gdouble_none_2 resample_funcs[7]
 632
 633 #define resample_gint16_linear_1 resample_funcs[8]
 634 #define resample_gint32_linear_1 resample_funcs[9]
 635 #define resample_gfloat_linear_1 resample_funcs[10]
 636 #define resample_gdouble_linear_1 resample_funcs[11]
 637
 638 #if defined HAVE_ORC && !defined DISABLE_ORC
 639 # if defined (__i386__) || defined (__x86_64__)
 640 #  define CHECK_X86
 641 #  include "audio-resampler-x86.h"
 642 # endif
 643 #endif
 644
 645 static void
 646 audio_resampler_init (void)
 647 {
 648   static gsize init_gonce = 0;
 649
 650   if (g_once_init_enter (&init_gonce)) {
 651
 652     GST_DEBUG_CATEGORY_INIT (audio_resampler_debug, "audio-resampler", 0,
 653         "audio-resampler object");
 654
 655 #if defined HAVE_ORC && !defined DISABLE_ORC
 656     orc_init ();
 657     {
 658       OrcTarget *target = orc_target_get_default ();
 659       gint i;
 660
 661       if (target) {
 662         unsigned int flags = orc_target_get_default_flags (target);
 663         const gchar *name;
 664
 665         name = orc_target_get_name (target);
 666         GST_DEBUG ("target %s, default flags %08x", name, flags);
 667
 668         for (i = 0; i < 32; ++i) {
 669           if (flags & (1U << i)) {
 670             name = orc_target_get_flag_name (target, i);
 671             GST_DEBUG ("target flag %s", name);
 672 #ifdef CHECK_X86
 673             audio_resampler_check_x86 (name);
 674 #endif
 675           }
 676         }
 677       }
 678     }
 679 #endif
 680     g_once_init_leave (&init_gonce, 1);
 681   }
 682 }
 683
 684 #define MAKE_DEINTERLEAVE_FUNC(type)                                    \
 685 static void                                                             \
 686 deinterleave_ ##type (GstAudioResampler * resampler, gpointer sbuf[],   \
 687     gpointer in[], gsize in_frames)                                     \
 688 {                                                                       \
 689   guint i, c, channels = resampler->channels;                           \
 690   gsize samples_avail = resampler->samples_avail;                       \
 691   for (c = 0; c < channels; c++) {                                      \
 692     type *s = (type *) sbuf[c] + samples_avail;                         \
 693     if (G_UNLIKELY (in == NULL)) {                                      \
 694       for (i = 0; i < in_frames; i++)                                   \
 695         s[i] = 0;                                                       \
 696     } else {                                                            \
 697       type *ip = (type *) in[0] + c;                                    \
 698       for (i = 0; i < in_frames; i++, ip += channels)                   \
 699         s[i] = *ip;                                                     \
 700     }                                                                   \
 701   }                                                                     \
 702 }
 703
 704 MAKE_DEINTERLEAVE_FUNC (gdouble);
 705 MAKE_DEINTERLEAVE_FUNC (gfloat);
 706 MAKE_DEINTERLEAVE_FUNC (gint32);
 707 MAKE_DEINTERLEAVE_FUNC (gint16);
 708
 709 static DeinterleaveFunc deinterleave_funcs[] = {
 710   deinterleave_gint16,
 711   deinterleave_gint32,
 712   deinterleave_gfloat,
 713   deinterleave_gdouble
 714 };
 715
 716 static void
 717 deinterleave_copy (GstAudioResampler * resampler, gpointer sbuf[],
 718     gpointer in[], gsize in_frames)
 719 {
 720   guint c, blocks = resampler->blocks;
 721   gsize bytes_avail, in_bytes, bpf;
 722
 723   bpf = resampler->bps * resampler->inc;
 724   bytes_avail = resampler->samples_avail * bpf;
 725   in_bytes = in_frames * bpf;
 726
 727   for (c = 0; c < blocks; c++) {
 728     if (G_UNLIKELY (in == NULL))
 729       memset ((guint8 *) sbuf[c] + bytes_avail, 0, in_bytes);
 730     else
 731       memcpy ((guint8 *) sbuf[c] + bytes_avail, in[c], in_bytes);
 732   }
 733 }
 734
 735 static void
 736 calculate_kaiser_params (GstAudioResampler * resampler)
 737 {
 738   gdouble A, B, dw, tr_bw, Fc;
 739   gint n;
 740   const KaiserQualityMap *q = &kaiser_qualities[DEFAULT_QUALITY];
 741
 742   /* default cutoff */
 743   Fc = q->cutoff;
 744   if (resampler->out_rate < resampler->in_rate)
 745     Fc *= q->downsample_cutoff_factor;
 746
 747   Fc = GET_OPT_CUTOFF (resampler->options, Fc);
 748   A = GET_OPT_STOP_ATTENUATION (resampler->options, q->stopband_attenuation);
 749   tr_bw =
 750       GET_OPT_TRANSITION_BANDWIDTH (resampler->options,
 751       q->transition_bandwidth);
 752
 753   GST_LOG ("Fc %f, A %f, tr_bw %f", Fc, A, tr_bw);
 754
 755   /* calculate Beta */
 756   if (A > 50)
 757     B = 0.1102 * (A - 8.7);
 758   else if (A >= 21)
 759     B = 0.5842 * pow (A - 21, 0.4) + 0.07886 * (A - 21);
 760   else
 761     B = 0.0;
 762   /* calculate transition width in radians */
 763   dw = 2 * G_PI * (tr_bw);
 764   /* order of the filter */
 765   n = (A - 8.0) / (2.285 * dw);
 766
 767   resampler->kaiser_beta = B;
 768   resampler->n_taps = n + 1;
 769   resampler->cutoff = Fc;
 770
 771   GST_LOG ("using Beta %f n_taps %d cutoff %f", resampler->kaiser_beta,
 772       resampler->n_taps, resampler->cutoff);
 773 }
 774
 775 static gboolean
 776 alloc_coeff_mem (GstAudioResampler * resampler, gint bps, gint n_taps,
 777     gint n_phases)
 778 {
 779   if (resampler->alloc_taps >= n_taps && resampler->alloc_phases >= n_phases)
 780     return FALSE;
 781
 782   resampler->tmpcoeff =
 783       g_realloc_n (resampler->tmpcoeff, n_taps, sizeof (gdouble));
 784
 785   resampler->cstride = GST_ROUND_UP_32 (bps * (n_taps + TAPS_OVERREAD));
 786   g_free (resampler->coeffmem);
 787   resampler->coeffmem = g_malloc0 (n_phases * resampler->cstride + ALIGN - 1);
 788   resampler->coeff = MEM_ALIGN (resampler->coeffmem, ALIGN);
 789   resampler->alloc_taps = n_taps;
 790   resampler->alloc_phases = n_phases;
 791
 792   return TRUE;
 793 }
 794
 795 static void
 796 resampler_calculate_taps (GstAudioResampler * resampler)
 797 {
 798   gint bps;
 799   gint n_taps;
 800   gint out_rate;
 801   gint in_rate, index, oversample;
 802   gboolean non_interleaved, interpolate;
 803   DeinterleaveFunc deinterleave;
 804   ResampleFunc resample, resample_2;
 805
 806   switch (resampler->method) {
 807     case GST_AUDIO_RESAMPLER_METHOD_NEAREST:
 808       resampler->n_taps = 2;
 809       break;
 810     case GST_AUDIO_RESAMPLER_METHOD_LINEAR:
 811       resampler->n_taps = GET_OPT_N_TAPS (resampler->options, 2);
 812       break;
 813     case GST_AUDIO_RESAMPLER_METHOD_CUBIC:
 814       resampler->n_taps = GET_OPT_N_TAPS (resampler->options, 4);
 815       resampler->b = GET_OPT_CUBIC_B (resampler->options);
 816       resampler->c = GET_OPT_CUBIC_C (resampler->options);;
 817       break;
 818     case GST_AUDIO_RESAMPLER_METHOD_BLACKMAN_NUTTALL:
 819     {
 820       const BlackmanQualityMap *q = &blackman_qualities[DEFAULT_QUALITY];
 821       resampler->n_taps = GET_OPT_N_TAPS (resampler->options, q->n_taps);
 822       resampler->cutoff = GET_OPT_CUTOFF (resampler->options, q->cutoff);
 823       break;
 824     }
 825     case GST_AUDIO_RESAMPLER_METHOD_KAISER:
 826       calculate_kaiser_params (resampler);
 827       break;
 828   }
 829
 830   in_rate = resampler->in_rate;
 831   out_rate = resampler->out_rate;
 832
 833   oversample = GET_OPT_FILTER_OVERSAMPLE (resampler->options);
 834
 835   if (out_rate < in_rate) {
 836     gint mult = 2;
 837
 838     resampler->cutoff = resampler->cutoff * out_rate / in_rate;
 839     resampler->n_taps =
 840         gst_util_uint64_scale_int (resampler->n_taps, in_rate, out_rate);
 841
 842     while (oversample > 1) {
 843       if (mult * out_rate >= in_rate)
 844         break;
 845
 846       mult *= 2;
 847       oversample >>= 1;
 848     }
 849   }
 850   resampler->oversample = oversample;
 851
 852   /* only round up for bigger taps, the small taps are used for nearest,
 853    * linear and cubic and we want to use less taps for those. */
 854   if (resampler->n_taps > 4)
 855     resampler->n_taps = GST_ROUND_UP_8 (resampler->n_taps);
 856
 857   n_taps = resampler->n_taps;
 858   bps = resampler->bps;
 859
 860   GST_LOG ("using n_taps %d cutoff %f, oversample %d", n_taps,
 861       resampler->cutoff, oversample);
 862
 863   resampler->filter_mode = GET_OPT_FILTER_MODE (resampler->options);
 864   resampler->filter_threshold =
 865       GET_OPT_FILTER_MODE_THRESHOLD (resampler->options);
 866
 867   switch (resampler->filter_mode) {
 868     case GST_AUDIO_RESAMPLER_FILTER_MODE_INTERPOLATED:
 869       interpolate = TRUE;
 870       break;
 871     case GST_AUDIO_RESAMPLER_FILTER_MODE_FULL:
 872       interpolate = FALSE;
 873       break;
 874     default:
 875     case GST_AUDIO_RESAMPLER_FILTER_MODE_AUTO:
 876       if (out_rate <= oversample) {
 877         /* don't interpolate if we need to calculate at least the same amount
 878          * of filter coefficients than the full table case */
 879         interpolate = FALSE;
 880       } else {
 881         interpolate = TRUE;
 882       }
 883       break;
 884   }
 885
 886   if (interpolate) {
 887     gint otaps = oversample * n_taps + 1;
 888     GstAudioResamplerFilterInterpolation filter_interpolation =
 889         GET_OPT_FILTER_INTERPOLATION (resampler->options);
 890
 891     /* if we're asked to intepolate but no interpolation was given, */
 892     if (filter_interpolation == GST_AUDIO_RESAMPLER_FILTER_INTERPOLATION_NONE)
 893       resampler->filter_interpolation = DEFAULT_OPT_FILTER_INTERPOLATION;
 894     else
 895       resampler->filter_interpolation = filter_interpolation;
 896
 897     if (alloc_coeff_mem (resampler, bps, otaps, 1)) {
 898       gpointer coeff = (gint8 *) resampler->coeff;
 899       gdouble *tmpcoeff = resampler->tmpcoeff;
 900       gdouble x, weight;
 901
 902       x = 1.0 - n_taps / 2;
 903       weight = fill_taps (resampler, tmpcoeff, x, otaps, oversample);
 904
 905       switch (resampler->format) {
 906         case GST_AUDIO_FORMAT_S16:
 907           convert_taps_gint16 (tmpcoeff, coeff, weight / oversample, otaps);
 908           break;
 909         case GST_AUDIO_FORMAT_S32:
 910           convert_taps_gint32 (tmpcoeff, coeff, weight / oversample, otaps);
 911           break;
 912         case GST_AUDIO_FORMAT_F32:
 913           convert_taps_gfloat (tmpcoeff, coeff, weight / oversample, otaps);
 914           break;
 915         default:
 916         case GST_AUDIO_FORMAT_F64:
 917           convert_taps_gdouble (tmpcoeff, coeff, weight / oversample, otaps);
 918           break;
 919       }
 920     }
 921   } else {
 922     resampler->filter_interpolation =
 923         GST_AUDIO_RESAMPLER_FILTER_INTERPOLATION_NONE;
 924     resampler->taps = g_realloc_n (resampler->taps, out_rate, sizeof (Tap));
 925     memset (resampler->taps, 0, sizeof (Tap) * out_rate);
 926     alloc_coeff_mem (resampler, bps, n_taps, out_rate);
 927   }
 928
 929   resampler->samp_inc = in_rate / out_rate;
 930   resampler->samp_frac = in_rate % out_rate;
 931
 932   non_interleaved =
 933       (resampler->flags & GST_AUDIO_RESAMPLER_FLAG_NON_INTERLEAVED);
 934
 935   resampler->ostride = non_interleaved ? 1 : resampler->channels;
 936
 937   switch (resampler->format) {
 938     case GST_AUDIO_FORMAT_S16:
 939       index = 0;
 940       break;
 941     case GST_AUDIO_FORMAT_S32:
 942       index = 1;
 943       break;
 944     case GST_AUDIO_FORMAT_F32:
 945       index = 2;
 946       break;
 947     case GST_AUDIO_FORMAT_F64:
 948       index = 3;
 949       break;
 950     default:
 951       g_assert_not_reached ();
 952       break;
 953   }
 954   deinterleave = deinterleave_funcs[index];
 955
 956   switch (resampler->filter_interpolation) {
 957     default:
 958     case GST_AUDIO_RESAMPLER_FILTER_INTERPOLATION_NONE:
 959       break;
 960     case GST_AUDIO_RESAMPLER_FILTER_INTERPOLATION_LINEAR:
 961       index += 8;
 962       break;
 963     case GST_AUDIO_RESAMPLER_FILTER_INTERPOLATION_CUBIC:
 964       index += 16;
 965       break;
 966   }
 967   resample = resample_funcs[index];
 968   resample_2 = resample_funcs[index + 4];
 969
 970   if (!non_interleaved && resampler->channels == 2 && n_taps >= 4 && resample_2) {
 971     /* we resample 2 channels in parallel */
 972     resampler->resample = resample_2;
 973     resampler->deinterleave = deinterleave_copy;
 974     resampler->blocks = 1;
 975     resampler->inc = resampler->channels;;
 976   } else {
 977     /* we resample each channel separately */
 978     resampler->resample = resample;
 979     resampler->deinterleave = deinterleave;
 980     resampler->blocks = resampler->channels;
 981     resampler->inc = 1;
 982   }
 983 }
 984
 985 #define PRINT_TAPS(type,print)                          \
 986 G_STMT_START {                                          \
 987   type sum = 0.0, *taps;                                \
 988   type icoeff[4];                                       \
 989   gint samp_index = 0, samp_phase = i;                  \
 990                                                         \
 991   taps = get_taps_##type##_none (resampler, &samp_index,\
 992       &samp_phase, icoeff);                             \
 993                                                         \
 994   for (j = 0; j < n_taps; j++) {                        \
 995     type tap = taps[j];                                 \
 996     fprintf (stderr, "\t%" print " ", tap);             \
 997     sum += tap;                                         \
 998   }                                                     \
 999   fprintf (stderr, "\t: sum %" print "\n", sum);        \
1000 } G_STMT_END
1001
1002 static void
1003 resampler_dump (GstAudioResampler * resampler)
1004 {
1005 #if 0
1006   gint i, n_taps, out_rate;
1007   gint64 a;
1008
1009   out_rate = resampler->out_rate;
1010   n_taps = resampler->n_taps;
1011
1012   fprintf (stderr, "out size %d, max taps %d\n", out_rate, n_taps);
1013
1014   a = g_get_monotonic_time ();
1015
1016   for (i = 0; i < out_rate; i++) {
1017     gint j;
1018
1019     //fprintf (stderr, "%u: %d %d\t ", i, t->sample_inc, t->next_phase);
1020     switch (resampler->format) {
1021       case GST_AUDIO_FORMAT_F64:
1022         PRINT_TAPS (gdouble, "f");
1023         break;
1024       case GST_AUDIO_FORMAT_F32:
1025         PRINT_TAPS (gfloat, "f");
1026         break;
1027       case GST_AUDIO_FORMAT_S32:
1028         PRINT_TAPS (gint32, "d");
1029         break;
1030       case GST_AUDIO_FORMAT_S16:
1031         PRINT_TAPS (gint16, "d");
1032         break;
1033       default:
1034         break;
1035     }
1036   }
1037   fprintf (stderr, "time %" G_GUINT64_FORMAT "\n", g_get_monotonic_time () - a);
1038 #endif
1039 }
1040
1041 /**
1042  * gst_audio_resampler_options_set_quality:
1043  * @method: a #GstAudioResamplerMethod
1044  * @quality: the quality
1045  * @in_rate: the input rate
1046  * @out_rate: the output rate
1047  * @options: a #GstStructure
1048  *
1049  * Set the parameters for resampling from @in_rate to @out_rate using @method
1050  * for @quality in @options.
1051  */
1052 void
1053 gst_audio_resampler_options_set_quality (GstAudioResamplerMethod method,
1054     guint quality, gint in_rate, gint out_rate, GstStructure * options)
1055 {
1056   g_return_if_fail (options != NULL);
1057   g_return_if_fail (quality <= GST_AUDIO_RESAMPLER_QUALITY_MAX);
1058   g_return_if_fail (in_rate > 0 && out_rate > 0);
1059
1060   switch (method) {
1061     case GST_AUDIO_RESAMPLER_METHOD_NEAREST:
1062       break;
1063     case GST_AUDIO_RESAMPLER_METHOD_LINEAR:
1064       gst_structure_set (options,
1065           GST_AUDIO_RESAMPLER_OPT_N_TAPS, G_TYPE_INT, 2, NULL);
1066       break;
1067     case GST_AUDIO_RESAMPLER_METHOD_CUBIC:
1068       gst_structure_set (options,
1069           GST_AUDIO_RESAMPLER_OPT_N_TAPS, G_TYPE_INT, 4,
1070           GST_AUDIO_RESAMPLER_OPT_CUBIC_B, G_TYPE_DOUBLE, DEFAULT_OPT_CUBIC_B,
1071           GST_AUDIO_RESAMPLER_OPT_CUBIC_C, G_TYPE_DOUBLE, DEFAULT_OPT_CUBIC_C,
1072           NULL);
1073       break;
1074     case GST_AUDIO_RESAMPLER_METHOD_BLACKMAN_NUTTALL:
1075     {
1076       const BlackmanQualityMap *map = &blackman_qualities[quality];
1077       gst_structure_set (options,
1078           GST_AUDIO_RESAMPLER_OPT_N_TAPS, G_TYPE_INT, map->n_taps,
1079           GST_AUDIO_RESAMPLER_OPT_CUTOFF, G_TYPE_DOUBLE, map->cutoff, NULL);
1080       break;
1081     }
1082     case GST_AUDIO_RESAMPLER_METHOD_KAISER:
1083     {
1084       const KaiserQualityMap *map = &kaiser_qualities[quality];
1085       gdouble cutoff;
1086
1087       cutoff = map->cutoff;
1088       if (out_rate < in_rate)
1089         cutoff *= map->downsample_cutoff_factor;
1090
1091       gst_structure_set (options,
1092           GST_AUDIO_RESAMPLER_OPT_CUTOFF, G_TYPE_DOUBLE, cutoff,
1093           GST_AUDIO_RESAMPLER_OPT_STOP_ATTENUATION, G_TYPE_DOUBLE,
1094           map->stopband_attenuation,
1095           GST_AUDIO_RESAMPLER_OPT_TRANSITION_BANDWIDTH, G_TYPE_DOUBLE,
1096           map->transition_bandwidth, NULL);
1097       break;
1098     }
1099   }
1100   gst_structure_set (options,
1101       GST_AUDIO_RESAMPLER_OPT_FILTER_OVERSAMPLE, G_TYPE_INT,
1102       oversample_qualities[quality], NULL);
1103 }
1104
1105 /**
1106  * gst_audio_resampler_new:
1107  * @resampler: a #GstAudioResampler
1108  * @method: a #GstAudioResamplerMethod
1109  * @flags: #GstAudioResamplerFlags
1110  * @in_rate: input rate
1111  * @out_rate: output rate
1112  * @options: extra options
1113  *
1114  * Make a new resampler.
1115  *
1116  * Returns: %TRUE on success
1117  */
1118 GstAudioResampler *
1119 gst_audio_resampler_new (GstAudioResamplerMethod method,
1120     GstAudioResamplerFlags flags,
1121     GstAudioFormat format, gint channels,
1122     gint in_rate, gint out_rate, GstStructure * options)
1123 {
1124   GstAudioResampler *resampler;
1125   const GstAudioFormatInfo *info;
1126
1127   g_return_val_if_fail (channels > 0, FALSE);
1128   g_return_val_if_fail (in_rate > 0, FALSE);
1129   g_return_val_if_fail (out_rate > 0, FALSE);
1130
1131   audio_resampler_init ();
1132
1133   resampler = g_slice_new0 (GstAudioResampler);
1134   resampler->method = method;
1135   resampler->flags = flags;
1136   resampler->format = format;
1137   resampler->channels = channels;
1138
1139   info = gst_audio_format_get_info (format);
1140   resampler->bps = GST_AUDIO_FORMAT_INFO_WIDTH (info) / 8;
1141   resampler->sbuf = g_malloc0 (sizeof (gpointer) * channels);
1142
1143   GST_DEBUG ("method %d, bps %d, channels %d", method, resampler->bps,
1144       resampler->channels);
1145
1146   gst_audio_resampler_update (resampler, in_rate, out_rate, options);
1147
1148   /* half of the filter is filled with 0 */
1149   resampler->samp_index = 0;
1150   resampler->samples_avail = resampler->n_taps / 2 - 1;
1151
1152   return resampler;
1153 }
1154
1155 /* make the buffers to hold the (deinterleaved) samples */
1156 static inline gpointer *
1157 get_sample_bufs (GstAudioResampler * resampler, gsize need)
1158 {
1159   if (G_LIKELY (resampler->samples_len < need)) {
1160     guint c, blocks = resampler->blocks;
1161     gsize bytes, to_move = 0;
1162     gint8 *ptr, *samples;
1163
1164     GST_LOG ("realloc %d -> %d", (gint) resampler->samples_len, (gint) need);
1165
1166     bytes = GST_ROUND_UP_N (need * resampler->bps * resampler->inc, ALIGN);
1167
1168     samples = g_malloc0 (blocks * bytes + ALIGN - 1);
1169     ptr = MEM_ALIGN (samples, ALIGN);
1170
1171     /* if we had some data, move history */
1172     if (resampler->samples_len > 0)
1173       to_move = resampler->samples_avail * resampler->bps * resampler->inc;
1174
1175     /* set up new pointers */
1176     for (c = 0; c < blocks; c++) {
1177       memcpy (ptr + (c * bytes), resampler->sbuf[c], to_move);
1178       resampler->sbuf[c] = ptr + (c * bytes);
1179     }
1180     g_free (resampler->samples);
1181     resampler->samples = samples;
1182     resampler->samples_len = need;
1183   }
1184   return resampler->sbuf;
1185 }
1186
1187 /**
1188  * gst_audio_resampler_reset:
1189  * @resampler: a #GstAudioResampler
1190  *
1191  * Reset @resampler to the state it was when it was first created, discarding
1192  * all sample history.
1193  */
1194 void
1195 gst_audio_resampler_reset (GstAudioResampler * resampler)
1196 {
1197   g_return_if_fail (resampler != NULL);
1198
1199   if (resampler->samples) {
1200     gsize bytes;
1201     gint c, blocks, bpf;
1202
1203     bpf = resampler->bps * resampler->inc;
1204     bytes = (resampler->n_taps / 2) * bpf;
1205     blocks = resampler->blocks;
1206
1207     for (c = 0; c < blocks; c++)
1208       memset (resampler->sbuf[c], 0, bytes);
1209   }
1210   /* half of the filter is filled with 0 */
1211   resampler->samp_index = 0;
1212   resampler->samples_avail = resampler->n_taps / 2 - 1;
1213 }
1214
1215 /**
1216  * gst_audio_resampler_update:
1217  * @resampler: a #GstAudioResampler
1218  * @in_rate: new input rate
1219  * @out_rate: new output rate
1220  * @options: new options or %NULL
1221  *
1222  * Update the resampler parameters for @resampler. This function should
1223  * not be called concurrently with any other function on @resampler.
1224  *
1225  * When @in_rate or @out_rate is 0, its value is unchanged.
1226  *
1227  * When @options is %NULL, the previously configured options are reused.
1228  *
1229  * Returns: %TRUE if the new parameters could be set
1230  */
1231 gboolean
1232 gst_audio_resampler_update (GstAudioResampler * resampler,
1233     gint in_rate, gint out_rate, GstStructure * options)
1234 {
1235   gint gcd, samp_phase, old_n_taps;
1236   gdouble max_error;
1237
1238   g_return_val_if_fail (resampler != NULL, FALSE);
1239
1240   if (in_rate <= 0)
1241     in_rate = resampler->in_rate;
1242   if (out_rate <= 0)
1243     out_rate = resampler->out_rate;
1244
1245   if (resampler->out_rate > 0)
1246     samp_phase =
1247         gst_util_uint64_scale_int (resampler->samp_phase, out_rate,
1248         resampler->out_rate);
1249   else
1250     samp_phase = 0;
1251
1252   gcd = gst_util_greatest_common_divisor (in_rate, out_rate);
1253
1254   max_error = GET_OPT_MAX_PHASE_ERROR (resampler->options);
1255
1256   if (max_error < 1.0e-8) {
1257     gcd = gst_util_greatest_common_divisor (gcd, samp_phase);
1258   } else {
1259     while (gcd > 1) {
1260       gdouble ph1 = (gdouble) samp_phase / out_rate;
1261       gint factor = 2;
1262
1263       /* reduce the factor until we have a phase error of less than 10% */
1264       gdouble ph2 = (gdouble) (samp_phase / gcd) / (out_rate / gcd);
1265
1266       if (fabs (ph1 - ph2) < max_error)
1267         break;
1268
1269       while (gcd % factor != 0)
1270         factor++;
1271       gcd /= factor;
1272
1273       GST_INFO ("divide by factor %d, gcd %d", factor, gcd);
1274     }
1275   }
1276
1277   GST_INFO ("phase %d, out_rate %d, in_rate %d, gcd %d", samp_phase, out_rate,
1278       in_rate, gcd);
1279
1280   resampler->samp_phase = samp_phase / gcd;
1281   resampler->in_rate = in_rate / gcd;
1282   resampler->out_rate = out_rate / gcd;
1283
1284   if (options) {
1285     if (resampler->options)
1286       gst_structure_free (resampler->options);
1287     resampler->options = gst_structure_copy (options);
1288   }
1289
1290   old_n_taps = resampler->n_taps;
1291
1292   resampler_calculate_taps (resampler);
1293   resampler_dump (resampler);
1294
1295   GST_DEBUG ("rate %u->%u, taps %d->%d", resampler->in_rate,
1296       resampler->out_rate, old_n_taps, resampler->n_taps);
1297
1298   if (old_n_taps > 0) {
1299     gpointer *sbuf;
1300     gint i, bpf, bytes, soff, doff, diff;
1301
1302     sbuf = get_sample_bufs (resampler, resampler->n_taps);
1303
1304     bpf = resampler->bps * resampler->inc;
1305     bytes = resampler->samples_avail * bpf;
1306     soff = doff = resampler->samp_index * bpf;
1307
1308     diff = ((gint) resampler->n_taps - old_n_taps) / 2;
1309
1310     if (diff < 0) {
1311       /* diff < 0, decrease taps, adjust source */
1312       soff += -diff * bpf;
1313       bytes -= -diff * bpf;
1314     } else {
1315       /* diff > 0, increase taps, adjust dest */
1316       doff += diff * bpf;
1317     }
1318
1319     /* now shrink or enlarge the history buffer, when we enlarge we
1320      * just leave the old samples in there. FIXME, probably do something better
1321      * like mirror or fill with zeroes. */
1322     for (i = 0; i < resampler->blocks; i++)
1323       memmove ((gint8 *) sbuf[i] + doff, (gint8 *) sbuf[i] + soff, bytes);
1324
1325     resampler->samples_avail += diff;
1326   }
1327   return TRUE;
1328 }
1329
1330 /**
1331  * gst_audio_resampler_free:
1332  * @resampler: a #GstAudioResampler
1333  *
1334  * Free a previously allocated #GstAudioResampler @resampler.
1335  *
1336  * Since: 1.6
1337  */
1338 void
1339 gst_audio_resampler_free (GstAudioResampler * resampler)
1340 {
1341   g_return_if_fail (resampler != NULL);
1342
1343   g_free (resampler->taps);
1344   g_free (resampler->coeffmem);
1345   g_free (resampler->tmpcoeff);
1346   g_free (resampler->samples);
1347   g_free (resampler->sbuf);
1348   if (resampler->options)
1349     gst_structure_free (resampler->options);
1350   g_slice_free (GstAudioResampler, resampler);
1351 }
1352
1353 static inline gsize
1354 calc_out (GstAudioResampler * resampler, gsize in)
1355 {
1356   gsize out;
1357
1358   out = in * resampler->out_rate;
1359   if (out < resampler->samp_phase)
1360     return 0;
1361
1362   out = ((out - resampler->samp_phase) / resampler->in_rate) + 1;
1363   GST_LOG ("out %d = ((%d * %d - %d) / %d) + 1", (gint) out,
1364       (gint) in, resampler->out_rate, resampler->samp_phase,
1365       resampler->in_rate);
1366
1367   return out;
1368 }
1369
1370 /**
1371  * gst_audio_resampler_get_out_frames:
1372  * @resampler: a #GstAudioResampler
1373  * @in_frames: number of input frames
1374  *
1375  * Get the number of output frames that would be currently available when
1376  * @in_frames are given to @resampler.
1377  *
1378  * Returns: The number of frames that would be availabe after giving
1379  * @in_frames as input to @resampler.
1380  */
1381 gsize
1382 gst_audio_resampler_get_out_frames (GstAudioResampler * resampler,
1383     gsize in_frames)
1384 {
1385   gsize need, avail;
1386
1387   g_return_val_if_fail (resampler != NULL, 0);
1388
1389   need = resampler->n_taps + resampler->samp_index + resampler->skip;
1390   avail = resampler->samples_avail + in_frames;
1391   GST_LOG ("need %d = %d + %d + %d, avail %d = %d + %d", (gint) need,
1392       resampler->n_taps, resampler->samp_index, resampler->skip,
1393       (gint) avail, (gint) resampler->samples_avail, (gint) in_frames);
1394   if (avail < need)
1395     return 0;
1396
1397   return calc_out (resampler, avail - need);
1398 }
1399
1400 /**
1401  * gst_audio_resampler_get_in_frames:
1402  * @resampler: a #GstAudioResampler
1403  * @out_frames: number of input frames
1404  *
1405  * Get the number of input frames that would currently be needed
1406  * to produce @out_frames from @resampler.
1407  *
1408  * Returns: The number of input frames needed for producing
1409  * @out_frames of data from @resampler.
1410  */
1411 gsize
1412 gst_audio_resampler_get_in_frames (GstAudioResampler * resampler,
1413     gsize out_frames)
1414 {
1415   gsize in_frames;
1416
1417   g_return_val_if_fail (resampler != NULL, 0);
1418
1419   in_frames =
1420       (resampler->samp_phase +
1421       out_frames * resampler->samp_frac) / resampler->out_rate;
1422   in_frames += out_frames * resampler->samp_inc;
1423
1424   return in_frames;
1425 }
1426
1427 /**
1428  * gst_audio_resampler_get_max_latency:
1429  * @resampler: a #GstAudioResampler
1430  *
1431  * Get the maximum number of input samples that the resampler would
1432  * need before producing output.
1433  *
1434  * Returns: the latency of @resampler as expressed in the number of
1435  * frames.
1436  */
1437 gsize
1438 gst_audio_resampler_get_max_latency (GstAudioResampler * resampler)
1439 {
1440   g_return_val_if_fail (resampler != NULL, 0);
1441
1442   return resampler->n_taps / 2;
1443 }
1444
1445 /**
1446  * gst_audio_resampler_resample:
1447  * @resampler: a #GstAudioResampler
1448  * @in: input samples
1449  * @in_frames: number of input frames
1450  * @out: output samples
1451  * @out_frames: number of output frames
1452  *
1453  * Perform resampling on @in_frames frames in @in and write @out_frames to @out.
1454  *
1455  * In case the samples are interleaved, @in and @out must point to an
1456  * array with a single element pointing to a block of interleaved samples.
1457  *
1458  * If non-interleaved samples are used, @in and @out must point to an
1459  * array with pointers to memory blocks, one for each channel.
1460  *
1461  * @in may be %NULL, in which case @in_frames of silence samples are pushed
1462  * into the resampler.
1463  *
1464  * This function always produces @out_frames of output and consumes @in_frames of
1465  * input. Use gst_audio_resampler_get_out_frames() and
1466  * gst_audio_resampler_get_in_frames() to make sure @in_frames and @out_frames
1467  * are matching and @in and @out point to enough memory.
1468  */
1469 void
1470 gst_audio_resampler_resample (GstAudioResampler * resampler,
1471     gpointer in[], gsize in_frames, gpointer out[], gsize out_frames)
1472 {
1473   gsize samples_avail;
1474   gsize need, consumed;
1475   gpointer *sbuf;
1476
1477   /* do sample skipping */
1478   if (G_UNLIKELY (resampler->skip >= in_frames)) {
1479     /* we need tp skip all input */
1480     resampler->skip -= in_frames;
1481     return;
1482   }
1483   /* skip the last samples by advancing the sample index */
1484   resampler->samp_index += resampler->skip;
1485
1486   samples_avail = resampler->samples_avail;
1487
1488   /* make sure we have enough space to copy our samples */
1489   sbuf = get_sample_bufs (resampler, in_frames + samples_avail);
1490
1491   /* copy/deinterleave the samples */
1492   resampler->deinterleave (resampler, sbuf, in, in_frames);
1493
1494   /* update new amount of samples in our buffer */
1495   resampler->samples_avail = samples_avail += in_frames;
1496
1497   need = resampler->n_taps + resampler->samp_index;
1498   if (G_UNLIKELY (samples_avail < need)) {
1499     /* not enough samples to start */
1500     return;
1501   }
1502
1503   /* resample all channels */
1504   resampler->resample (resampler, sbuf, samples_avail, out, out_frames,
1505       &consumed);
1506
1507   GST_LOG ("in %" G_GSIZE_FORMAT ", avail %" G_GSIZE_FORMAT ", consumed %"
1508       G_GSIZE_FORMAT, in_frames, samples_avail, consumed);
1509
1510   /* update pointers */
1511   if (G_LIKELY (consumed > 0)) {
1512     gssize left = samples_avail - consumed;
1513     if (left > 0) {
1514       /* we consumed part of our samples */
1515       resampler->samples_avail = left;
1516     } else {
1517       /* we consumed all our samples, empty our buffers */
1518       resampler->samples_avail = 0;
1519       resampler->skip = -left;
1520     }
1521   }
1522 }