gst-libs/gst/audio/audio-quantize.c

   1 /* GStreamer
   2  * Copyright (C) 2007 Sebastian Dröge <slomo@circular-chaos.org>
   3  *           (C) 2015 Wim Taymans <wim.taymans@gmail.com>
   4  *
   5  * gstaudioquantize.c: quantizes audio to the target format and optionally
   6  *                     applies dithering and noise shaping.
   7  *
   8  * This library is free software; you can redistribute it and/or
   9  * modify it under the terms of the GNU Library General Public
  10  * License as published by the Free Software Foundation; either
  11  * version 2 of the License, or (at your option) any later version.
  12  *
  13  * This library is distributed in the hope that it will be useful,
  14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16  * Library General Public License for more details.
  17  *
  18  * You should have received a copy of the GNU Library General Public
  19  * License along with this library; if not, write to the
  20  * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
  21  * Boston, MA 02110-1301, USA.
  22  */
  23
  24 /* TODO: - Maybe drop 5-pole noise shaping and use coefficients
  25  *         generated by dmaker
  26  *         http://shibatch.sf.net
  27  */
  28
  29 #include <gst/gst.h>
  30 #include <string.h>
  31 #include <math.h>
  32
  33 #include "gstaudiopack.h"
  34 #include "audio-quantize.h"
  35
  36 typedef void (*QuantizeFunc) (GstAudioQuantize * quant, const gpointer src,
  37     gpointer dst, gint count);
  38
  39 struct _GstAudioQuantize
  40 {
  41   GstAudioDitherMethod dither;
  42   GstAudioNoiseShapingMethod ns;
  43   GstAudioQuantizeFlags flags;
  44   GstAudioFormat format;
  45   guint quantizer;
  46   guint stride;
  47   guint blocks;
  48
  49   guint shift;
  50   guint32 mask, bias;
  51
  52   /* last random number generated per channel for hifreq TPDF dither */
  53   gpointer last_random;
  54   /* contains the past quantization errors, error[channels][count] */
  55   guint error_size;
  56   gpointer error_buf;
  57   /* buffer with dither values */
  58   guint dither_size;
  59   gpointer dither_buf;
  60   /* noise shaping coefficients */
  61   gpointer coeffs;
  62   gint n_coeffs;
  63
  64   QuantizeFunc quantize;
  65 };
  66
  67 #define ADDSS(res,val) \
  68         if (val > 0 && res > 0 && G_MAXINT32 - res <= val){             \
  69           res = G_MAXINT32;                                             \
  70         } else if (val < 0 && res < 0 && G_MININT32 - res >= val){      \
  71           res = G_MININT32;                                             \
  72         } else                                                          \
  73           res += val;
  74
  75 static void
  76 gst_audio_quantize_quantize_memcpy (GstAudioQuantize * quant,
  77     const gpointer src, gpointer dst, gint samples)
  78 {
  79   if (src != dst)
  80     memcpy (dst, src, samples * sizeof (gint32) * quant->stride);
  81 }
  82
  83 /* Quantize functions for gint32 as intermediate format */
  84 static void
  85 gst_audio_quantize_quantize_int_none_none (GstAudioQuantize * quant,
  86     const gpointer src, gpointer dst, gint samples)
  87 {
  88   audio_orc_int_bias (dst, src, quant->bias, ~quant->mask,
  89       samples * quant->stride);
  90 }
  91
  92 /* This is the base function, implementing a linear congruential generator
  93  * and returning a pseudo random number between 0 and 2^32 - 1.
  94  */
  95 static inline guint32
  96 gst_fast_random_uint32 (void)
  97 {
  98   static guint32 state = 0xdeadbeef;
  99   return (state = state * 1103515245 + 12345);
 100 }
 101
 102 static inline gint32
 103 gst_fast_random_int32 (void)
 104 {
 105   return (gint32) gst_fast_random_uint32 ();
 106 }
 107
 108 /* Assuming dither == 2^n,
 109  * returns one of 2^(n+1) possible random values:
 110  * -dither <= retval < dither */
 111 #define RANDOM_INT_DITHER(dither)                                       \
 112   (- dither + (gst_fast_random_int32 () & ((dither << 1) - 1)))
 113
 114 static void
 115 setup_dither_buf (GstAudioQuantize * quant, gint samples)
 116 {
 117   gboolean need_init = FALSE;
 118   gint stride = quant->stride;
 119   gint i, len = samples * stride;
 120   guint shift = quant->shift;
 121   guint32 bias;
 122   gint32 dither, *d;
 123
 124   if (quant->dither_size < len) {
 125     quant->dither_size = len;
 126     quant->dither_buf = g_realloc (quant->dither_buf, len * sizeof (gint32));
 127     need_init = TRUE;
 128   }
 129
 130   bias = quant->bias;
 131   d = quant->dither_buf;
 132
 133   switch (quant->dither) {
 134     case GST_AUDIO_DITHER_NONE:
 135       if (need_init) {
 136         for (i = 0; i < len; i++)
 137           d[i] = 0;
 138       }
 139       break;
 140
 141     case GST_AUDIO_DITHER_RPDF:
 142       dither = 1 << (shift);
 143       for (i = 0; i < len; i++)
 144         d[i] = bias + RANDOM_INT_DITHER (dither);
 145       break;
 146
 147     case GST_AUDIO_DITHER_TPDF:
 148       dither = 1 << (shift - 1);
 149       for (i = 0; i < len; i++)
 150         d[i] = bias + RANDOM_INT_DITHER (dither) + RANDOM_INT_DITHER (dither);
 151       break;
 152
 153     case GST_AUDIO_DITHER_TPDF_HF:
 154     {
 155       gint32 tmp, *last_random = quant->last_random;
 156
 157       dither = 1 << (shift - 1);
 158       for (i = 0; i < len; i++) {
 159         tmp = RANDOM_INT_DITHER (dither);
 160         d[i] = bias + tmp - last_random[i % stride];
 161         last_random[i % stride] = tmp;
 162       }
 163       break;
 164     }
 165   }
 166 }
 167
 168 static void
 169 gst_audio_quantize_quantize_int_dither_none (GstAudioQuantize * quant,
 170     const gpointer src, gpointer dst, gint samples)
 171 {
 172   setup_dither_buf (quant, samples);
 173
 174   audio_orc_int_dither (dst, src, quant->dither_buf, ~quant->mask,
 175       samples * quant->stride);
 176 }
 177
 178 static void
 179 setup_error_buf (GstAudioQuantize * quant, gint samples, gint extra)
 180 {
 181   gint stride = quant->stride;
 182   gint len = (samples + extra) * stride;
 183
 184   if (quant->error_size < len) {
 185     quant->error_buf = g_realloc (quant->error_buf, len * sizeof (gint32));
 186     if (quant->error_size == 0)
 187       memset ((gint32 *) quant->error_buf, 0, stride * extra * sizeof (gint32));
 188     quant->error_size = len;
 189   }
 190 }
 191
 192 static void
 193 gst_audio_quantize_quantize_int_dither_feedback (GstAudioQuantize * quant,
 194     const gpointer src, gpointer dst, gint samples)
 195 {
 196   guint32 mask;
 197   gint i, len, stride;
 198   const gint32 *s = src;
 199   gint32 *dith, *d = dst, v, o, *e, err;
 200
 201   setup_dither_buf (quant, samples);
 202   setup_error_buf (quant, samples, 1);
 203
 204   stride = quant->stride;
 205   len = samples * stride;
 206   dith = quant->dither_buf;
 207   e = quant->error_buf;
 208   mask = ~quant->mask;
 209
 210   for (i = 0; i < len; i++) {
 211     o = v = s[i];
 212     /* add dither */
 213     err = dith[i];
 214     /* remove error */
 215     err -= e[i];
 216     ADDSS (v, err);
 217     v &= mask;
 218     /* store new error */
 219     e[i + stride] = e[i] + (v - o);
 220     /* store result */
 221     d[i] = v;
 222   }
 223   memmove (e, &e[len], sizeof (gint32) * stride);
 224 }
 225
 226 #define SHIFT 10
 227 #define REDUCE 8
 228 #define RROUND (1<<(REDUCE-1))
 229 #define SREDUCE 2
 230 #define SROUND (1<<(SREDUCE-1))
 231
 232 static void
 233 gst_audio_quantize_quantize_int_dither_noise_shape (GstAudioQuantize * quant,
 234     const gpointer src, gpointer dst, gint samples)
 235 {
 236   guint32 mask;
 237   gint i, j, k, len, stride, nc;
 238   const gint32 *s = src;
 239   gint32 *c, *dith, *d = dst, v, o, *e, err;
 240
 241   nc = quant->n_coeffs;
 242
 243   setup_dither_buf (quant, samples);
 244   setup_error_buf (quant, samples, nc);
 245
 246   stride = quant->stride;
 247   len = samples * stride;
 248   dith = quant->dither_buf;
 249   e = quant->error_buf;
 250   c = quant->coeffs;
 251   mask = ~quant->mask;
 252
 253   for (i = 0; i < len; i++) {
 254     v = s[i];
 255     /* combine and remove error */
 256     err = 0;
 257     for (j = 0, k = i; j < nc; j++, k += stride)
 258       err -= e[k] * c[j];
 259     err = (err + SROUND) >> (SREDUCE);
 260     ADDSS (v, err);
 261     o = v;
 262     /* add dither */
 263     err = dith[i];
 264     ADDSS (v, err);
 265     /* quantize */
 266     v &= mask;
 267     /* store new error with reduced precision */
 268     e[k] = (v - o + RROUND) >> REDUCE;
 269     /* store result */
 270     d[i] = v;
 271   }
 272   memmove (e, &e[len], sizeof (gint32) * stride * nc);
 273 }
 274
 275 #define MAKE_QUANTIZE_FUNC_NAME(name)                                   \
 276 gst_audio_quantize_quantize_##name
 277
 278 static const QuantizeFunc quantize_funcs[] = {
 279   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_none_none),
 280   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_feedback),
 281   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_noise_shape),
 282   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_noise_shape),
 283   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_noise_shape),
 284   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_none),
 285   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_feedback),
 286   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_noise_shape),
 287   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_noise_shape),
 288   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_noise_shape),
 289   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_none),
 290   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_feedback),
 291   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_noise_shape),
 292   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_noise_shape),
 293   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_noise_shape),
 294   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_none),
 295   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_feedback),
 296   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_noise_shape),
 297   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_noise_shape),
 298   (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (int_dither_noise_shape),
 299 };
 300
 301 /* Same as error feedback but also add 1/2 of the previous error value.
 302  * This moves the noise a bit more into the higher frequencies. */
 303 static const gdouble ns_simple_coeffs[] = {
 304   -0.5, 1.0
 305 };
 306
 307 /* Noise shaping coefficients from[1], moves most power of the
 308  * error noise into inaudible frequency ranges.
 309  *
 310  * [1]
 311  * "Minimally Audible Noise Shaping", Stanley P. Lipshitz,
 312  * John Vanderkooy, and Robert A. Wannamaker,
 313  * J. Audio Eng. Soc., Vol. 39, No. 11, November 1991. */
 314
 315 static const gdouble ns_medium_coeffs[] = {
 316   0.6149, -1.590, 1.959, -2.165, 2.033
 317 };
 318
 319 /* Noise shaping coefficients by David Schleef, moves most power of the
 320  * error noise into inaudible frequency ranges */
 321 static const gdouble ns_high_coeffs[] = {
 322   -0.340122, 0.876066, -1.72008, 2.61339, -3.31399, 3.27918, -2.92975, 2.08484,
 323 };
 324
 325
 326 static void
 327 gst_audio_quantize_setup_noise_shaping (GstAudioQuantize * quant)
 328 {
 329   gint i, n_coeffs = 0;
 330   gint32 *q;
 331   const gdouble *coeffs;
 332
 333   switch (quant->ns) {
 334     case GST_AUDIO_NOISE_SHAPING_HIGH:
 335       n_coeffs = 8;
 336       coeffs = ns_high_coeffs;
 337       break;
 338
 339     case GST_AUDIO_NOISE_SHAPING_MEDIUM:
 340       n_coeffs = 5;
 341       coeffs = ns_medium_coeffs;
 342       break;
 343
 344     case GST_AUDIO_NOISE_SHAPING_SIMPLE:
 345       n_coeffs = 2;
 346       coeffs = ns_simple_coeffs;
 347       break;
 348
 349     case GST_AUDIO_NOISE_SHAPING_ERROR_FEEDBACK:
 350       break;
 351
 352     case GST_AUDIO_NOISE_SHAPING_NONE:
 353     default:
 354       break;
 355   }
 356
 357   if (n_coeffs) {
 358     quant->n_coeffs = n_coeffs;
 359     q = quant->coeffs = g_new0 (gint32, n_coeffs);
 360     for (i = 0; i < n_coeffs; i++)
 361       q[i] = floor (coeffs[i] * (1 << SHIFT) + 0.5);
 362   }
 363   return;
 364 }
 365
 366 static void
 367 gst_audio_quantize_setup_dither (GstAudioQuantize * quant)
 368 {
 369   switch (quant->dither) {
 370     case GST_AUDIO_DITHER_TPDF_HF:
 371       quant->last_random = g_new0 (gint32, quant->stride);
 372       break;
 373     case GST_AUDIO_DITHER_RPDF:
 374     case GST_AUDIO_DITHER_TPDF:
 375       quant->last_random = NULL;
 376       break;
 377     case GST_AUDIO_DITHER_NONE:
 378     default:
 379       quant->last_random = NULL;
 380       break;
 381   }
 382   return;
 383 }
 384
 385 static void
 386 gst_audio_quantize_setup_quantize_func (GstAudioQuantize * quant)
 387 {
 388   gint index;
 389
 390   if (quant->shift == 0) {
 391     quant->quantize = (QuantizeFunc) MAKE_QUANTIZE_FUNC_NAME (memcpy);
 392     return;
 393   }
 394
 395   index = 5 * quant->dither + quant->ns;
 396   quant->quantize = quantize_funcs[index];
 397 }
 398
 399 static gint
 400 count_power (guint v)
 401 {
 402   gint res = 0;
 403   while (v > 1) {
 404     res++;
 405     v >>= 1;
 406   }
 407   return res;
 408 }
 409
 410 /**
 411  * gst_audio_quantize_new: (skip):
 412  * @dither: a #GstAudioDitherMethod
 413  * @ns: a #GstAudioNoiseShapingMethod
 414  * @flags: #GstAudioQuantizeFlags
 415  * @format: the #GstAudioFormat of the samples
 416  * @channels: the amount of channels in the samples
 417  * @quantizer: the quantizer to use
 418  *
 419  * Create a new quantizer object with the given parameters.
 420  *
 421  * Output samples will be quantized to a multiple of @quantizer. Better
 422  * performance is achieved when @quantizer is a power of 2.
 423  *
 424  * Dithering and noise-shaping can be performed during quantization with
 425  * the @dither and @ns parameters.
 426  *
 427  * Returns: a new #GstAudioQuantize. Free with gst_audio_quantize_free().
 428  */
 429 GstAudioQuantize *
 430 gst_audio_quantize_new (GstAudioDitherMethod dither,
 431     GstAudioNoiseShapingMethod ns, GstAudioQuantizeFlags flags,
 432     GstAudioFormat format, guint channels, guint quantizer)
 433 {
 434   GstAudioQuantize *quant;
 435
 436   g_return_val_if_fail (format == GST_AUDIO_FORMAT_S32, NULL);
 437   g_return_val_if_fail (channels > 0, NULL);
 438
 439   quant = g_slice_new0 (GstAudioQuantize);
 440   quant->dither = dither;
 441   quant->ns = ns;
 442   quant->flags = flags;
 443   quant->format = format;
 444   if (flags & GST_AUDIO_QUANTIZE_FLAG_NON_INTERLEAVED) {
 445     quant->stride = 1;
 446     quant->blocks = channels;
 447   } else {
 448     quant->stride = channels;
 449     quant->blocks = 1;
 450   }
 451   quant->quantizer = quantizer;
 452
 453   quant->shift = count_power (quantizer);
 454   if (quant->shift > 0)
 455     quant->bias = (1U << (quant->shift - 1));
 456   else
 457     quant->bias = 0;
 458   quant->mask = (1U << quant->shift) - 1;
 459
 460   gst_audio_quantize_setup_dither (quant);
 461   gst_audio_quantize_setup_noise_shaping (quant);
 462   gst_audio_quantize_setup_quantize_func (quant);
 463
 464   return quant;
 465 }
 466
 467 /**
 468  * gst_audio_quantize_free:
 469  * @quant: a #GstAudioQuantize
 470  *
 471  * Free a #GstAudioQuantize.
 472  */
 473 void
 474 gst_audio_quantize_free (GstAudioQuantize * quant)
 475 {
 476   g_return_if_fail (quant != NULL);
 477
 478   g_free (quant->error_buf);
 479   g_free (quant->coeffs);
 480   g_free (quant->last_random);
 481   g_free (quant->dither_buf);
 482
 483   g_slice_free (GstAudioQuantize, quant);
 484 }
 485
 486 /**
 487  * gst_audio_quantize_reset:
 488  * @quant: a #GstAudioQuantize
 489  *
 490  * Reset @quant to the state is was when created, clearing any
 491  * history it might have.
 492  */
 493 void
 494 gst_audio_quantize_reset (GstAudioQuantize * quant)
 495 {
 496   g_free (quant->error_buf);
 497   quant->error_buf = NULL;
 498   quant->error_size = 0;
 499 }
 500
 501 /**
 502  * gst_audio_quantize_samples:
 503  * @quant: a #GstAudioQuantize
 504  * @in: input samples
 505  * @out: output samples
 506  * @samples: number of samples
 507  *
 508  * Perform quantization on @samples in @in and write the result to @out.
 509  *
 510  * In case the samples are interleaved, @in and @out must point to an
 511  * array with a single element pointing to a block of interleaved samples.
 512  *
 513  * If non-interleaved samples are used, @in and @out must point to an
 514  * array with pointers to memory blocks, one for each channel.
 515  *
 516  * @in and @out may point to the same memory location, in which case samples will be
 517  * modified in-place.
 518  */
 519 void
 520 gst_audio_quantize_samples (GstAudioQuantize * quant,
 521     const gpointer in[], gpointer out[], guint samples)
 522 {
 523   guint i;
 524
 525   g_return_if_fail (quant != NULL);
 526   g_return_if_fail (out != NULL || samples == 0);
 527   g_return_if_fail (in != NULL || samples == 0);
 528
 529   for (i = 0; i < quant->blocks; i++)
 530     quant->quantize (quant, in[i], out[i], samples);
 531 }