gst/equalizer/gstiirequalizer.c

   1 /* GStreamer
   2  * Copyright (C) <2004> Benjamin Otte <otte@gnome.org>
   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., 59 Temple Place - Suite 330,
  17  * Boston, MA 02111-1307, USA.
  18  */
  19
  20 #ifdef HAVE_CONFIG_H
  21 #include "config.h"
  22 #endif
  23
  24 #include <math.h>
  25 #include <string.h>
  26 #include <gst/gst.h>
  27 #include <gst/audio/audio.h>
  28 #include <gst/audio/gstaudiofilter.h>
  29
  30 typedef struct _GstIirEqualizer GstIirEqualizer;
  31 typedef struct _GstIirEqualizerClass GstIirEqualizerClass;
  32
  33 #define GST_TYPE_IIR_EQUALIZER \
  34   (gst_iir_equalizer_get_type())
  35 #define GST_IIR_EQUALIZER(obj) \
  36   (G_TYPE_CHECK_INSTANCE_CAST((obj),GST_TYPE_IIR_EQUALIZER,GstIirEqualizer))
  37 #define GST_IIR_EQUALIZER_CLASS(klass) \
  38   (G_TYPE_CHECK_CLASS_CAST((klass),GST_TYPE_IIR_EQUALIZER,GstIirEqualizerClass))
  39 #define GST_IS_IIR_EQUALIZER(obj) \
  40   (G_TYPE_CHECK_INSTANCE_TYPE((obj),GST_TYPE_IIR_EQUALIZER))
  41 #define GST_IS_IIR_EQUALIZER_CLASS(obj) \
  42   (G_TYPE_CHECK_CLASS_TYPE((klass),GST_TYPE_IIR_EQUALIZER))
  43
  44 #define LOWEST_FREQ (20.0)
  45 #define HIGHEST_FREQ (20000.0)
  46
  47 typedef void (*ProcessFunc) (GstIirEqualizer * equ, guint8 * data, guint size,
  48     guint channels);
  49
  50 typedef struct
  51 {
  52   gdouble alpha;                /* IIR coefficients for outputs */
  53   gdouble beta;                 /* IIR coefficients for inputs */
  54   gdouble gamma;                /* IIR coefficients for inputs */
  55 } SecondOrderFilter;
  56
  57 struct _GstIirEqualizer
  58 {
  59   GstAudiofilter audiofilter;
  60
  61   /* properties */
  62   guint freq_count;
  63   gdouble bandwidth;
  64   gdouble *freqs;
  65   gdouble *values;
  66
  67   /* data */
  68   SecondOrderFilter *filter;
  69   gpointer history;
  70   ProcessFunc process;
  71   guint history_size;
  72 };
  73
  74 struct _GstIirEqualizerClass
  75 {
  76   GstAudiofilterClass audiofilter_class;
  77 };
  78
  79 enum
  80 {
  81   ARG_0,
  82   ARG_BANDS,
  83   ARG_BANDWIDTH,
  84   ARG_VALUES
  85       /* FILL ME */
  86 };
  87
  88 static void gst_iir_equalizer_base_init (gpointer g_class);
  89 static void gst_iir_equalizer_class_init (gpointer g_class,
  90     gpointer class_data);
  91 static void gst_iir_equalizer_init (GTypeInstance * instance, gpointer g_class);
  92 static void gst_iir_equalizer_finalize (GObject * object);
  93
  94 static void gst_iir_equalizer_set_property (GObject * object,
  95     guint prop_id, const GValue * value, GParamSpec * pspec);
  96 static void gst_iir_equalizer_get_property (GObject * object,
  97     guint prop_id, GValue * value, GParamSpec * pspec);
  98
  99 static void gst_iir_equalizer_setup (GstAudiofilter * iir_equalizer);
 100 static void gst_iir_equalizer_filter_inplace (GstAudiofilter *
 101     iir_equalizer, GstBuffer * buf);
 102
 103 static GstAudiofilterClass *parent_class;
 104
 105 GType
 106 gst_iir_equalizer_get_type (void)
 107 {
 108   static GType iir_equalizer_type = 0;
 109
 110   if (!iir_equalizer_type) {
 111     static const GTypeInfo iir_equalizer_info = {
 112       sizeof (GstIirEqualizerClass),
 113       gst_iir_equalizer_base_init,
 114       NULL,
 115       gst_iir_equalizer_class_init,
 116       NULL,
 117       gst_iir_equalizer_init,
 118       sizeof (GstIirEqualizer),
 119       0,
 120       NULL,
 121     };
 122
 123     iir_equalizer_type = g_type_register_static (GST_TYPE_AUDIOFILTER,
 124         "GstIirEqualizer", &iir_equalizer_info, 0);
 125   }
 126   return iir_equalizer_type;
 127 }
 128
 129 static void
 130 gst_iir_equalizer_base_init (gpointer g_class)
 131 {
 132   static GstElementDetails iir_equalizer_details = {
 133     "Equalizer",
 134     "Filter/Effect/Audio",
 135     "Direct Form IIR equalizer",
 136     "Benjamin Otte <otte@gnome.org>"
 137   };
 138   GstIirEqualizerClass *klass = (GstIirEqualizerClass *) g_class;
 139   GstElementClass *element_class = GST_ELEMENT_CLASS (klass);
 140   GstCaps *caps;
 141
 142   gst_element_class_set_details (element_class, &iir_equalizer_details);
 143
 144   caps = gst_caps_from_string ("audio/x-raw-int, depth=(int)16, width=(int)16, "
 145       "endianness=(int)BYTE_ORDER, signed=(bool)TRUE, "
 146       "rate=(int)[1000,MAX], channels=(int)[1,6];"
 147       "audio/x-raw-float, width=(int)32, endianness=(int)BYTE_ORDER,"
 148       "rate=(int)[1000,MAX], channels=(int)[1,6]");
 149   gst_audiofilter_class_add_pad_templates (GST_AUDIOFILTER_CLASS (g_class),
 150       caps);
 151   gst_caps_free (caps);
 152 }
 153
 154 static void
 155 gst_iir_equalizer_class_init (gpointer g_class, gpointer class_data)
 156 {
 157   GObjectClass *gobject_class;
 158   GstElementClass *gstelement_class;
 159   GstIirEqualizerClass *klass;
 160   GstAudiofilterClass *audiofilter_class;
 161
 162   klass = (GstIirEqualizerClass *) g_class;
 163   gobject_class = (GObjectClass *) klass;
 164   gstelement_class = (GstElementClass *) klass;
 165   audiofilter_class = (GstAudiofilterClass *) g_class;
 166
 167   gobject_class->set_property = gst_iir_equalizer_set_property;
 168   gobject_class->get_property = gst_iir_equalizer_get_property;
 169   gobject_class->finalize = gst_iir_equalizer_finalize;
 170
 171   parent_class = g_type_class_peek_parent (g_class);
 172
 173   g_object_class_install_property (gobject_class, ARG_BANDS,
 174       g_param_spec_uint ("bands", "bands", "number of different bands to use",
 175           2, 64, 15, G_PARAM_READWRITE | G_PARAM_CONSTRUCT));
 176   g_object_class_install_property (gobject_class, ARG_BANDWIDTH,
 177       g_param_spec_double ("bandwidth", "bandwidth",
 178           "bandwidth calculated as distance between bands * this value", 0.1,
 179           5.0, 1.0, G_PARAM_READWRITE | G_PARAM_CONSTRUCT));
 180   /* FIXME FIXME FIXME */
 181   g_object_class_install_property (gobject_class, ARG_VALUES,
 182       g_param_spec_pointer ("values", "values",
 183           "expects a gdouble* of values to use for the bands",
 184           G_PARAM_WRITABLE));
 185
 186   audiofilter_class->setup = gst_iir_equalizer_setup;
 187   audiofilter_class->filter_inplace = gst_iir_equalizer_filter_inplace;
 188 }
 189
 190 static void
 191 gst_iir_equalizer_init (GTypeInstance * instance, gpointer g_class)
 192 {
 193 }
 194
 195 static void
 196 gst_iir_equalizer_finalize (GObject * object)
 197 {
 198   GstIirEqualizer *equ = GST_IIR_EQUALIZER (object);
 199
 200   g_free (equ->freqs);
 201   g_free (equ->values);
 202   g_free (equ->filter);
 203   g_free (equ->history);
 204
 205   G_OBJECT_CLASS (parent_class)->finalize (object);
 206 }
 207
 208 /* args are in the range [-1 ... 1] with 0 meaning "no action"
 209  * convert to [-0.2 ... 1] with 0 meaning no action via the function
 210  * f(x) = 0.25 * 5 ^ x - 0.25
 211  */
 212 static gdouble
 213 arg_to_scale (gdouble arg)
 214 {
 215   return 0.25 * exp (log (5) * arg) - 0.25;
 216 }
 217
 218 static void
 219 setup_filter (GstIirEqualizer * equ, SecondOrderFilter * filter, gdouble gain,
 220     gdouble frequency)
 221 {
 222   gdouble q = pow (HIGHEST_FREQ / LOWEST_FREQ,
 223       1.0 / (equ->freq_count - 1)) * equ->bandwidth;
 224   gdouble theta = frequency * 2 * M_PI;
 225
 226   filter->beta = (q - theta / 2) / (2 * q + theta);
 227   filter->gamma = (0.5 + filter->beta) * cos (theta);
 228   filter->alpha = (0.5 - filter->beta) / 2;
 229
 230   filter->beta *= 2.0;
 231   filter->alpha *= 2.0 * gain;
 232   filter->gamma *= 2.0;
 233   GST_INFO ("gain = %g, frequency = %g, alpha = %g, beta = %g, gamma=%g\n",
 234       gain, frequency, filter->alpha, filter->beta, filter->gamma);
 235 }
 236
 237 static void
 238 gst_iir_equalizer_compute_frequencies (GstIirEqualizer * equ, guint band_count)
 239 {
 240   gdouble *old_values;
 241   guint old_count, i;
 242   gdouble step = pow (HIGHEST_FREQ / LOWEST_FREQ, 1.0 / (band_count - 1));
 243   GstAudiofilter *audio = GST_AUDIOFILTER (equ);
 244
 245   old_count = equ->freq_count;
 246   equ->freq_count = band_count;
 247   old_values = equ->values;
 248   if (old_count < band_count) {
 249     equ->freqs = g_realloc (equ->freqs, sizeof (gdouble) * band_count);
 250     memset (equ->freqs + sizeof (gdouble) * old_count, 0,
 251         sizeof (gdouble) * (band_count - old_count));
 252     equ->values = g_realloc (equ->values, sizeof (gdouble) * band_count);
 253     memset (equ->values + sizeof (gdouble) * old_count, 0,
 254         sizeof (gdouble) * (band_count - old_count));
 255     equ->filter =
 256         g_realloc (equ->filter, sizeof (SecondOrderFilter) * band_count);
 257     memset (equ->filter + sizeof (SecondOrderFilter) * old_count, 0,
 258         sizeof (SecondOrderFilter) * (band_count - old_count));
 259   }
 260   equ->history =
 261       g_realloc (equ->history,
 262       equ->history_size * audio->channels * band_count);
 263   memset (equ->history, 0, equ->history_size * audio->channels * band_count);
 264   equ->freqs[0] = LOWEST_FREQ;
 265   for (i = 1; i < band_count; i++) {
 266     equ->freqs[i] = equ->freqs[i - 1] * step;
 267   }
 268
 269   if (audio->rate) {
 270     guint i;
 271
 272     for (i = 0; i < band_count; i++) {
 273       setup_filter (equ, &equ->filter[i], arg_to_scale (equ->values[i]),
 274           equ->freqs[i] / audio->rate);
 275     }
 276   }
 277 }
 278
 279 static void
 280 gst_iir_equalizer_set_property (GObject * object, guint prop_id,
 281     const GValue * value, GParamSpec * pspec)
 282 {
 283   GstIirEqualizer *equ = GST_IIR_EQUALIZER (object);
 284
 285   switch (prop_id) {
 286     case ARG_BANDS:
 287       gst_iir_equalizer_compute_frequencies (equ, g_value_get_uint (value));
 288       break;
 289     case ARG_BANDWIDTH:
 290       if (g_value_get_double (value) != equ->bandwidth) {
 291         equ->bandwidth = g_value_get_double (value);
 292         if (GST_AUDIOFILTER (equ)->rate) {
 293           guint i;
 294
 295           for (i = 0; i < equ->freq_count; i++) {
 296             setup_filter (equ, &equ->filter[i], arg_to_scale (equ->values[i]),
 297                 equ->freqs[i] / GST_AUDIOFILTER (equ)->rate);
 298           }
 299         }
 300       }
 301       break;
 302     case ARG_VALUES:
 303     {
 304       gdouble *new = g_value_get_pointer (value);
 305       guint i;
 306
 307       for (i = 0; i < equ->freq_count; i++) {
 308         if (new[i] != equ->values[i]) {
 309           equ->values[i] = new[i];
 310           setup_filter (equ, &equ->filter[i], arg_to_scale (new[i]),
 311               equ->freqs[i] / GST_AUDIOFILTER (equ)->rate);
 312         }
 313       }
 314     }
 315       break;
 316     default:
 317       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
 318       break;
 319   }
 320 }
 321
 322 static void
 323 gst_iir_equalizer_get_property (GObject * object, guint prop_id,
 324     GValue * value, GParamSpec * pspec)
 325 {
 326   GstIirEqualizer *equ = GST_IIR_EQUALIZER (object);
 327
 328   switch (prop_id) {
 329     case ARG_BANDS:
 330       g_value_set_uint (value, equ->freq_count);
 331       break;
 332     case ARG_BANDWIDTH:
 333       g_value_set_double (value, equ->bandwidth);
 334       break;
 335     default:
 336       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
 337       break;
 338   }
 339 }
 340
 341 /* start of code that is type specific */
 342
 343 #define CREATE_OPTIMIZED_FUNCTIONS(TYPE,BIG_TYPE,MIN_VAL,MAX_VAL)       \
 344 typedef struct {                                                        \
 345   TYPE x1, x2;          /* history of input values for a filter */      \
 346   TYPE y1, y2;          /* history of output values for a filter */     \
 347 } SecondOrderHistory ## TYPE;                                           \
 348                                                                         \
 349 static inline TYPE                                                      \
 350 one_step_ ## TYPE (SecondOrderFilter *filter,                           \
 351     SecondOrderHistory ## TYPE *history, TYPE input)                    \
 352 {                                                                       \
 353   /* calculate output */                                                \
 354   TYPE output = filter->alpha * (input - history->x2) +                 \
 355     filter->gamma * history->y1 - filter->beta * history->y2;           \
 356   /* update history */                                                  \
 357   history->y2 = history->y1;                                            \
 358   history->y1 = output;                                                 \
 359   history->x2 = history->x1;                                            \
 360   history->x1 = input;                                                  \
 361                                                                         \
 362   return output;                                                        \
 363 }                                                                       \
 364                                                                         \
 365 static const guint                                                      \
 366 history_size_ ## TYPE = sizeof (SecondOrderHistory ## TYPE);            \
 367                                                                         \
 368 static void                                                             \
 369 gst_iir_equ_process_ ## TYPE (GstIirEqualizer *equ, guint8 *data,       \
 370 guint size, guint channels)                                             \
 371 {                                                                       \
 372   guint frames = size / channels / sizeof (TYPE);                       \
 373   guint i, c, f;                                                        \
 374   BIG_TYPE cur;                                                         \
 375   TYPE val;                                                             \
 376                                                                         \
 377   for (i = 0; i < frames; i++) {                                        \
 378     for (c = 0; c < channels; c++) {                                    \
 379       SecondOrderHistory ## TYPE *history = equ->history;               \
 380       val = *((TYPE *) data);                                           \
 381       cur = 0;                                                          \
 382       for (f = 0; f < equ->freq_count; f++) {                           \
 383         SecondOrderFilter *filter = &equ->filter[f];                    \
 384                                                                         \
 385         cur += one_step_ ## TYPE (filter, history, val);                \
 386         history++;                                                      \
 387       }                                                                 \
 388       cur += val * 0.25;                                                \
 389       cur = CLAMP (cur, MIN_VAL, MAX_VAL);                              \
 390       *((TYPE *) data) = (TYPE) cur;                                    \
 391       data += sizeof (TYPE);                                            \
 392     }                                                                   \
 393   }                                                                     \
 394 }
 395
 396 CREATE_OPTIMIZED_FUNCTIONS (gint16, gint, -32768, 32767);
 397 CREATE_OPTIMIZED_FUNCTIONS (gfloat, gfloat, -1.0, 1.0);
 398
 399 static void
 400 gst_iir_equalizer_filter_inplace (GstAudiofilter * filter, GstBuffer * buf)
 401 {
 402   GstIirEqualizer *equ = GST_IIR_EQUALIZER (filter);
 403
 404   equ->process (equ, GST_BUFFER_DATA (buf), GST_BUFFER_SIZE (buf),
 405       filter->channels);
 406 }
 407
 408 static void
 409 gst_iir_equalizer_setup (GstAudiofilter * audio)
 410 {
 411   GstIirEqualizer *equ = GST_IIR_EQUALIZER (audio);
 412
 413   if (audio->width == 16) {
 414     equ->history_size = history_size_gint16;
 415     equ->process = gst_iir_equ_process_gint16;
 416   } else if (audio->width == 32) {
 417     equ->history_size = history_size_gfloat;
 418     equ->process = gst_iir_equ_process_gfloat;
 419   } else {
 420     g_assert_not_reached ();
 421   }
 422   gst_iir_equalizer_compute_frequencies (equ, equ->freq_count);
 423 }
 424
 425 static gboolean
 426 plugin_init (GstPlugin * plugin)
 427 {
 428   if (!gst_library_load ("gstaudiofilter"))
 429     return FALSE;
 430
 431   return gst_element_register (plugin, "equalizer", GST_RANK_NONE,
 432       GST_TYPE_IIR_EQUALIZER);
 433 }
 434
 435 GST_PLUGIN_DEFINE (GST_VERSION_MAJOR,
 436     GST_VERSION_MINOR,
 437     "equalizer",
 438     "GStreamer equalizers",
 439     plugin_init, VERSION, "LGPL", GST_PACKAGE_NAME, GST_PACKAGE_ORIGIN)