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       GST_ELEMENT_DETAILS ("Equalizer",
 134       "Filter/Effect/Audio",
 135       "Direct Form IIR equalizer",
 136       "Benjamin Otte <otte@gnome.org>");
 137   GstIirEqualizerClass *klass = (GstIirEqualizerClass *) g_class;
 138   GstElementClass *element_class = GST_ELEMENT_CLASS (klass);
 139   GstCaps *caps;
 140
 141   gst_element_class_set_details (element_class, &iir_equalizer_details);
 142
 143   caps = gst_caps_from_string ("audio/x-raw-int, depth=(int)16, width=(int)16, "
 144       "endianness=(int)BYTE_ORDER, signed=(bool)TRUE, "
 145       "rate=(int)[1000,MAX], channels=(int)[1,6];"
 146       "audio/x-raw-float, width=(int)32, endianness=(int)BYTE_ORDER,"
 147       "rate=(int)[1000,MAX], channels=(int)[1,6]");
 148   gst_audiofilter_class_add_pad_templates (GST_AUDIOFILTER_CLASS (g_class),
 149       caps);
 150   gst_caps_free (caps);
 151 }
 152
 153 static void
 154 gst_iir_equalizer_class_init (gpointer g_class, gpointer class_data)
 155 {
 156   GObjectClass *gobject_class;
 157   GstElementClass *gstelement_class;
 158   GstIirEqualizerClass *klass;
 159   GstAudiofilterClass *audiofilter_class;
 160
 161   klass = (GstIirEqualizerClass *) g_class;
 162   gobject_class = (GObjectClass *) klass;
 163   gstelement_class = (GstElementClass *) klass;
 164   audiofilter_class = (GstAudiofilterClass *) g_class;
 165
 166   gobject_class->set_property = gst_iir_equalizer_set_property;
 167   gobject_class->get_property = gst_iir_equalizer_get_property;
 168   gobject_class->finalize = gst_iir_equalizer_finalize;
 169
 170   parent_class = g_type_class_peek_parent (g_class);
 171
 172   g_object_class_install_property (gobject_class, ARG_BANDS,
 173       g_param_spec_uint ("bands", "bands", "number of different bands to use",
 174           2, 64, 15, G_PARAM_READWRITE | G_PARAM_CONSTRUCT));
 175   g_object_class_install_property (gobject_class, ARG_BANDWIDTH,
 176       g_param_spec_double ("bandwidth", "bandwidth",
 177           "bandwidth calculated as distance between bands * this value", 0.1,
 178           5.0, 1.0, G_PARAM_READWRITE | G_PARAM_CONSTRUCT));
 179   /* FIXME FIXME FIXME */
 180   g_object_class_install_property (gobject_class, ARG_VALUES,
 181       g_param_spec_pointer ("values", "values",
 182           "expects a gdouble* of values to use for the bands",
 183           G_PARAM_WRITABLE));
 184
 185   audiofilter_class->setup = gst_iir_equalizer_setup;
 186   audiofilter_class->filter_inplace = gst_iir_equalizer_filter_inplace;
 187 }
 188
 189 static void
 190 gst_iir_equalizer_init (GTypeInstance * instance, gpointer g_class)
 191 {
 192 }
 193
 194 static void
 195 gst_iir_equalizer_finalize (GObject * object)
 196 {
 197   GstIirEqualizer *equ = GST_IIR_EQUALIZER (object);
 198
 199   g_free (equ->freqs);
 200   g_free (equ->values);
 201   g_free (equ->filter);
 202   g_free (equ->history);
 203
 204   G_OBJECT_CLASS (parent_class)->finalize (object);
 205 }
 206
 207 /* args are in the range [-1 ... 1] with 0 meaning "no action"
 208  * convert to [-0.2 ... 1] with 0 meaning no action via the function
 209  * f(x) = 0.25 * 5 ^ x - 0.25
 210  */
 211 static gdouble
 212 arg_to_scale (gdouble arg)
 213 {
 214   return 0.25 * exp (log (5) * arg) - 0.25;
 215 }
 216
 217 static void
 218 setup_filter (GstIirEqualizer * equ, SecondOrderFilter * filter, gdouble gain,
 219     gdouble frequency)
 220 {
 221   gdouble q = pow (HIGHEST_FREQ / LOWEST_FREQ,
 222       1.0 / (equ->freq_count - 1)) * equ->bandwidth;
 223   gdouble theta = frequency * 2 * M_PI;
 224
 225   filter->beta = (q - theta / 2) / (2 * q + theta);
 226   filter->gamma = (0.5 + filter->beta) * cos (theta);
 227   filter->alpha = (0.5 - filter->beta) / 2;
 228
 229   filter->beta *= 2.0;
 230   filter->alpha *= 2.0 * gain;
 231   filter->gamma *= 2.0;
 232   GST_INFO ("gain = %g, frequency = %g, alpha = %g, beta = %g, gamma=%g\n",
 233       gain, frequency, filter->alpha, filter->beta, filter->gamma);
 234 }
 235
 236 static void
 237 gst_iir_equalizer_compute_frequencies (GstIirEqualizer * equ, guint band_count)
 238 {
 239   gdouble *old_values;
 240   guint old_count, i;
 241   gdouble step = pow (HIGHEST_FREQ / LOWEST_FREQ, 1.0 / (band_count - 1));
 242   GstAudiofilter *audio = GST_AUDIOFILTER (equ);
 243
 244   old_count = equ->freq_count;
 245   equ->freq_count = band_count;
 246   old_values = equ->values;
 247   if (old_count < band_count) {
 248     equ->freqs = g_realloc (equ->freqs, sizeof (gdouble) * band_count);
 249     memset (equ->freqs + sizeof (gdouble) * old_count, 0,
 250         sizeof (gdouble) * (band_count - old_count));
 251     equ->values = g_realloc (equ->values, sizeof (gdouble) * band_count);
 252     memset (equ->values + sizeof (gdouble) * old_count, 0,
 253         sizeof (gdouble) * (band_count - old_count));
 254     equ->filter =
 255         g_realloc (equ->filter, sizeof (SecondOrderFilter) * band_count);
 256     memset (equ->filter + sizeof (SecondOrderFilter) * old_count, 0,
 257         sizeof (SecondOrderFilter) * (band_count - old_count));
 258   }
 259   equ->history =
 260       g_realloc (equ->history,
 261       equ->history_size * audio->channels * band_count);
 262   memset (equ->history, 0, equ->history_size * audio->channels * band_count);
 263   equ->freqs[0] = LOWEST_FREQ;
 264   for (i = 1; i < band_count; i++) {
 265     equ->freqs[i] = equ->freqs[i - 1] * step;
 266   }
 267
 268   if (audio->rate) {
 269     guint i;
 270
 271     for (i = 0; i < band_count; i++) {
 272       setup_filter (equ, &equ->filter[i], arg_to_scale (equ->values[i]),
 273           equ->freqs[i] / audio->rate);
 274     }
 275   }
 276 }
 277
 278 static void
 279 gst_iir_equalizer_set_property (GObject * object, guint prop_id,
 280     const GValue * value, GParamSpec * pspec)
 281 {
 282   GstIirEqualizer *equ = GST_IIR_EQUALIZER (object);
 283
 284   switch (prop_id) {
 285     case ARG_BANDS:
 286       gst_iir_equalizer_compute_frequencies (equ, g_value_get_uint (value));
 287       break;
 288     case ARG_BANDWIDTH:
 289       if (g_value_get_double (value) != equ->bandwidth) {
 290         equ->bandwidth = g_value_get_double (value);
 291         if (GST_AUDIOFILTER (equ)->rate) {
 292           guint i;
 293
 294           for (i = 0; i < equ->freq_count; i++) {
 295             setup_filter (equ, &equ->filter[i], arg_to_scale (equ->values[i]),
 296                 equ->freqs[i] / GST_AUDIOFILTER (equ)->rate);
 297           }
 298         }
 299       }
 300       break;
 301     case ARG_VALUES:
 302     {
 303       gdouble *new = g_value_get_pointer (value);
 304       guint i;
 305
 306       for (i = 0; i < equ->freq_count; i++) {
 307         if (new[i] != equ->values[i]) {
 308           equ->values[i] = new[i];
 309           setup_filter (equ, &equ->filter[i], arg_to_scale (new[i]),
 310               equ->freqs[i] / GST_AUDIOFILTER (equ)->rate);
 311         }
 312       }
 313     }
 314       break;
 315     default:
 316       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
 317       break;
 318   }
 319 }
 320
 321 static void
 322 gst_iir_equalizer_get_property (GObject * object, guint prop_id,
 323     GValue * value, GParamSpec * pspec)
 324 {
 325   GstIirEqualizer *equ = GST_IIR_EQUALIZER (object);
 326
 327   switch (prop_id) {
 328     case ARG_BANDS:
 329       g_value_set_uint (value, equ->freq_count);
 330       break;
 331     case ARG_BANDWIDTH:
 332       g_value_set_double (value, equ->bandwidth);
 333       break;
 334     default:
 335       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
 336       break;
 337   }
 338 }
 339
 340 /* start of code that is type specific */
 341
 342 #define CREATE_OPTIMIZED_FUNCTIONS(TYPE,BIG_TYPE,MIN_VAL,MAX_VAL)       \
 343 typedef struct {                                                        \
 344   TYPE x1, x2;          /* history of input values for a filter */      \
 345   TYPE y1, y2;          /* history of output values for a filter */     \
 346 } SecondOrderHistory ## TYPE;                                           \
 347                                                                         \
 348 static inline TYPE                                                      \
 349 one_step_ ## TYPE (SecondOrderFilter *filter,                           \
 350     SecondOrderHistory ## TYPE *history, TYPE input)                    \
 351 {                                                                       \
 352   /* calculate output */                                                \
 353   TYPE output = filter->alpha * (input - history->x2) +                 \
 354     filter->gamma * history->y1 - filter->beta * history->y2;           \
 355   /* update history */                                                  \
 356   history->y2 = history->y1;                                            \
 357   history->y1 = output;                                                 \
 358   history->x2 = history->x1;                                            \
 359   history->x1 = input;                                                  \
 360                                                                         \
 361   return output;                                                        \
 362 }                                                                       \
 363                                                                         \
 364 static const guint                                                      \
 365 history_size_ ## TYPE = sizeof (SecondOrderHistory ## TYPE);            \
 366                                                                         \
 367 static void                                                             \
 368 gst_iir_equ_process_ ## TYPE (GstIirEqualizer *equ, guint8 *data,       \
 369 guint size, guint channels)                                             \
 370 {                                                                       \
 371   guint frames = size / channels / sizeof (TYPE);                       \
 372   guint i, c, f;                                                        \
 373   BIG_TYPE cur;                                                         \
 374   TYPE val;                                                             \
 375                                                                         \
 376   for (i = 0; i < frames; i++) {                                        \
 377     for (c = 0; c < channels; c++) {                                    \
 378       SecondOrderHistory ## TYPE *history = equ->history;               \
 379       val = *((TYPE *) data);                                           \
 380       cur = 0;                                                          \
 381       for (f = 0; f < equ->freq_count; f++) {                           \
 382         SecondOrderFilter *filter = &equ->filter[f];                    \
 383                                                                         \
 384         cur += one_step_ ## TYPE (filter, history, val);                \
 385         history++;                                                      \
 386       }                                                                 \
 387       cur += val * 0.25;                                                \
 388       cur = CLAMP (cur, MIN_VAL, MAX_VAL);                              \
 389       *((TYPE *) data) = (TYPE) cur;                                    \
 390       data += sizeof (TYPE);                                            \
 391     }                                                                   \
 392   }                                                                     \
 393 }
 394
 395 CREATE_OPTIMIZED_FUNCTIONS (gint16, gint, -32768, 32767);
 396 CREATE_OPTIMIZED_FUNCTIONS (gfloat, gfloat, -1.0, 1.0);
 397
 398 static void
 399 gst_iir_equalizer_filter_inplace (GstAudiofilter * filter, GstBuffer * buf)
 400 {
 401   GstIirEqualizer *equ = GST_IIR_EQUALIZER (filter);
 402
 403   equ->process (equ, GST_BUFFER_DATA (buf), GST_BUFFER_SIZE (buf),
 404       filter->channels);
 405 }
 406
 407 static void
 408 gst_iir_equalizer_setup (GstAudiofilter * audio)
 409 {
 410   GstIirEqualizer *equ = GST_IIR_EQUALIZER (audio);
 411
 412   if (audio->width == 16) {
 413     equ->history_size = history_size_gint16;
 414     equ->process = gst_iir_equ_process_gint16;
 415   } else if (audio->width == 32) {
 416     equ->history_size = history_size_gfloat;
 417     equ->process = gst_iir_equ_process_gfloat;
 418   } else {
 419     g_assert_not_reached ();
 420   }
 421   gst_iir_equalizer_compute_frequencies (equ, equ->freq_count);
 422 }
 423
 424 static gboolean
 425 plugin_init (GstPlugin * plugin)
 426 {
 427   if (!gst_library_load ("gstaudiofilter"))
 428     return FALSE;
 429
 430   return gst_element_register (plugin, "equalizer", GST_RANK_NONE,
 431       GST_TYPE_IIR_EQUALIZER);
 432 }
 433
 434 GST_PLUGIN_DEFINE (GST_VERSION_MAJOR,
 435     GST_VERSION_MINOR,
 436     "equalizer",
 437     "GStreamer equalizers",
 438     plugin_init, VERSION, "LGPL", GST_PACKAGE_NAME, GST_PACKAGE_ORIGIN)