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
  27 #include "gstiirequalizer.h"
  28
  29 #define GST_EQUALIZER_TRANSFORM_LOCK(eq) \
  30     g_mutex_lock (GST_BASE_TRANSFORM(eq)->transform_lock)
  31
  32 #define GST_EQUALIZER_TRANSFORM_UNLOCK(eq) \
  33     g_mutex_unlock (GST_BASE_TRANSFORM(eq)->transform_lock)
  34
  35 enum
  36 {
  37   ARG_0,
  38   ARG_NUM_BANDS,
  39   ARG_BAND_WIDTH,
  40   ARG_BAND_VALUES
  41 };
  42
  43 static void gst_iir_equalizer_finalize (GObject * object);
  44 static void gst_iir_equalizer_set_property (GObject * object,
  45     guint prop_id, const GValue * value, GParamSpec * pspec);
  46 static void gst_iir_equalizer_get_property (GObject * object,
  47     guint prop_id, GValue * value, GParamSpec * pspec);
  48
  49 static gboolean gst_iir_equalizer_setup (GstAudioFilter * filter,
  50     GstRingBufferSpec * fmt);
  51 static GstFlowReturn gst_iir_equalizer_transform_ip (GstBaseTransform * btrans,
  52     GstBuffer * buf);
  53
  54 GST_DEBUG_CATEGORY_STATIC (equalizer_debug);
  55 #define GST_CAT_DEFAULT equalizer_debug
  56
  57 #define ALLOWED_CAPS \
  58     "audio/x-raw-int,"                                                \
  59     " depth=(int)16,"                                                 \
  60     " width=(int)16,"                                                 \
  61     " endianness=(int)BYTE_ORDER,"                                    \
  62     " signed=(bool)TRUE,"                                             \
  63     " rate=(int)[1000,MAX],"                                          \
  64     " channels=(int)[1,MAX]; "                                        \
  65     "audio/x-raw-float,"                                              \
  66     " width=(int)32,"                                                 \
  67     " endianness=(int)BYTE_ORDER,"                                    \
  68     " rate=(int)[1000,MAX],"                                          \
  69     " channels=(int)[1,MAX]"
  70
  71 GST_BOILERPLATE (GstIirEqualizer, gst_iir_equalizer, GstAudioFilter,
  72     GST_TYPE_AUDIO_FILTER);
  73
  74 static void
  75 gst_iir_equalizer_base_init (gpointer g_class)
  76 {
  77   GstAudioFilterClass *audiofilter_class = GST_AUDIO_FILTER_CLASS (g_class);
  78   GstElementClass *element_class = GST_ELEMENT_CLASS (g_class);
  79   const GstElementDetails iir_equalizer_details =
  80       GST_ELEMENT_DETAILS ("Equalizer",
  81       "Filter/Effect/Audio",
  82       "Direct Form IIR equalizer",
  83       "Benjamin Otte <otte@gnome.org>");
  84   GstCaps *caps;
  85
  86   gst_element_class_set_details (element_class, &iir_equalizer_details);
  87
  88   caps = gst_caps_from_string (ALLOWED_CAPS);
  89   gst_audio_filter_class_add_pad_templates (audiofilter_class, caps);
  90   gst_caps_unref (caps);
  91 }
  92
  93 static void
  94 gst_iir_equalizer_class_init (GstIirEqualizerClass * klass)
  95 {
  96   GstAudioFilterClass *audio_filter_class = (GstAudioFilterClass *) klass;
  97   GstBaseTransformClass *btrans_class = (GstBaseTransformClass *) klass;
  98   GObjectClass *gobject_class = (GObjectClass *) klass;
  99
 100   gobject_class->set_property = gst_iir_equalizer_set_property;
 101   gobject_class->get_property = gst_iir_equalizer_get_property;
 102   gobject_class->finalize = gst_iir_equalizer_finalize;
 103
 104   g_object_class_install_property (gobject_class, ARG_NUM_BANDS,
 105       g_param_spec_uint ("num-bands", "num-bands",
 106           "number of different bands to use", 2, 64, 15,
 107           G_PARAM_READWRITE | G_PARAM_CONSTRUCT));
 108   g_object_class_install_property (gobject_class, ARG_BAND_WIDTH,
 109       g_param_spec_double ("band-width", "band-width",
 110           "band width calculated as distance between bands * this value", 0.1,
 111           5.0, 1.0, G_PARAM_READWRITE | G_PARAM_CONSTRUCT));
 112   g_object_class_install_property (gobject_class, ARG_BAND_VALUES,
 113       g_param_spec_value_array ("band-values", "band values",
 114           "GValueArray holding gdouble values, one for each band with values "
 115           "ranging from -1.0 to +1.0",
 116           g_param_spec_double ("band-value", "band-value",
 117               "Equaliser Band Value", -1.0, 1.0, 0.0, G_PARAM_WRITABLE),
 118           G_PARAM_WRITABLE));
 119
 120   audio_filter_class->setup = gst_iir_equalizer_setup;
 121   btrans_class->transform_ip = gst_iir_equalizer_transform_ip;
 122 }
 123
 124 static void
 125 gst_iir_equalizer_init (GstIirEqualizer * eq, GstIirEqualizerClass * g_class)
 126 {
 127   /* nothing to do here */
 128 }
 129
 130 static void
 131 gst_iir_equalizer_finalize (GObject * object)
 132 {
 133   GstIirEqualizer *equ = GST_IIR_EQUALIZER (object);
 134
 135   g_free (equ->freqs);
 136   g_free (equ->values);
 137   g_free (equ->filter);
 138   g_free (equ->history);
 139
 140   G_OBJECT_CLASS (parent_class)->finalize (object);
 141 }
 142
 143 /* args are in the range [-1 ... 1] with 0 meaning "no action"
 144  * convert to [-0.2 ... 1] with 0 meaning no action via the function
 145  * f(x) = 0.25 * 5 ^ x - 0.25
 146  */
 147 static gdouble
 148 arg_to_scale (gdouble arg)
 149 {
 150   return 0.25 * exp (log (5) * arg) - 0.25;
 151 }
 152
 153 static void
 154 setup_filter (GstIirEqualizer * equ, SecondOrderFilter * filter, gdouble gain,
 155     gdouble frequency)
 156 {
 157   gdouble q = pow (HIGHEST_FREQ / LOWEST_FREQ,
 158       1.0 / (equ->freq_count - 1)) * equ->band_width;
 159   gdouble theta = frequency * 2 * M_PI;
 160
 161   filter->beta = (q - theta / 2) / (2 * q + theta);
 162   filter->gamma = (0.5 + filter->beta) * cos (theta);
 163   filter->alpha = (0.5 - filter->beta) / 2;
 164
 165   filter->beta *= 2.0;
 166   filter->alpha *= 2.0 * gain;
 167   filter->gamma *= 2.0;
 168   GST_INFO ("gain = %g, frequency = %g, alpha = %g, beta = %g, gamma=%g\n",
 169       gain, frequency, filter->alpha, filter->beta, filter->gamma);
 170 }
 171
 172 static void
 173 gst_iir_equalizer_compute_frequencies (GstIirEqualizer * equ, guint band_count)
 174 {
 175   gdouble *old_values;
 176   guint old_count, i;
 177   gdouble step = pow (HIGHEST_FREQ / LOWEST_FREQ, 1.0 / (band_count - 1));
 178   GstAudioFilter *audio = GST_AUDIO_FILTER (equ);
 179
 180   old_count = equ->freq_count;
 181   equ->freq_count = band_count;
 182   old_values = equ->values;
 183   if (old_count < band_count) {
 184     equ->freqs = g_realloc (equ->freqs, sizeof (gdouble) * band_count);
 185     memset (equ->freqs + sizeof (gdouble) * old_count, 0,
 186         sizeof (gdouble) * (band_count - old_count));
 187     equ->values = g_realloc (equ->values, sizeof (gdouble) * band_count);
 188     memset (equ->values + sizeof (gdouble) * old_count, 0,
 189         sizeof (gdouble) * (band_count - old_count));
 190     equ->filter =
 191         g_realloc (equ->filter, sizeof (SecondOrderFilter) * band_count);
 192     memset (equ->filter + sizeof (SecondOrderFilter) * old_count, 0,
 193         sizeof (SecondOrderFilter) * (band_count - old_count));
 194   }
 195
 196   /* free + alloc = no memcpy */
 197   g_free (equ->history);
 198   equ->history =
 199       g_malloc0 (equ->history_size * audio->format.channels * band_count);
 200   equ->freqs[0] = LOWEST_FREQ;
 201   for (i = 1; i < band_count; i++) {
 202     equ->freqs[i] = equ->freqs[i - 1] * step;
 203   }
 204
 205   if (audio->format.rate > 0) {
 206     guint i;
 207
 208     for (i = 0; i < band_count; i++) {
 209       setup_filter (equ, &equ->filter[i], arg_to_scale (equ->values[i]),
 210           equ->freqs[i] / audio->format.rate);
 211     }
 212   }
 213 }
 214
 215 static void
 216 gst_iir_equalizer_set_property (GObject * object, guint prop_id,
 217     const GValue * value, GParamSpec * pspec)
 218 {
 219   GstIirEqualizer *equ = GST_IIR_EQUALIZER (object);
 220
 221   GST_EQUALIZER_TRANSFORM_LOCK (equ);
 222   GST_OBJECT_LOCK (equ);
 223   switch (prop_id) {
 224     case ARG_NUM_BANDS:
 225       gst_iir_equalizer_compute_frequencies (equ, g_value_get_uint (value));
 226       break;
 227     case ARG_BAND_WIDTH:
 228       if (g_value_get_double (value) != equ->band_width) {
 229         equ->band_width = g_value_get_double (value);
 230         if (GST_AUDIO_FILTER (equ)->format.rate) {
 231           guint i;
 232
 233           for (i = 0; i < equ->freq_count; i++) {
 234             setup_filter (equ, &equ->filter[i], arg_to_scale (equ->values[i]),
 235                 equ->freqs[i] / GST_AUDIO_FILTER (equ)->format.rate);
 236           }
 237         }
 238       }
 239       break;
 240     case ARG_BAND_VALUES:{
 241       GValueArray *arr;
 242
 243       arr = (GValueArray *) g_value_get_boxed (value);
 244       if (arr == NULL) {
 245         g_warning ("Application tried to set empty band value array");
 246       } else if (arr->n_values != equ->freq_count) {
 247         g_warning ("Application tried to set %u band values, but there are "
 248             "%u bands", arr->n_values, equ->freq_count);
 249       } else {
 250         guint i;
 251
 252         for (i = 0; i < arr->n_values; ++i) {
 253           gdouble new_val;
 254
 255           new_val = g_value_get_double (g_value_array_get_nth (arr, i));
 256           if (new_val != equ->values[i]) {
 257             equ->values[i] = new_val;
 258             setup_filter (equ, &equ->filter[i], arg_to_scale (new_val),
 259                 equ->freqs[i] / GST_AUDIO_FILTER (equ)->format.rate);
 260           }
 261         }
 262       }
 263       break;
 264     }
 265     default:
 266       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
 267       break;
 268   }
 269   GST_OBJECT_UNLOCK (equ);
 270   GST_EQUALIZER_TRANSFORM_UNLOCK (equ);
 271 }
 272
 273 static void
 274 gst_iir_equalizer_get_property (GObject * object, guint prop_id,
 275     GValue * value, GParamSpec * pspec)
 276 {
 277   GstIirEqualizer *equ = GST_IIR_EQUALIZER (object);
 278
 279   GST_EQUALIZER_TRANSFORM_LOCK (equ);
 280   GST_OBJECT_LOCK (equ);
 281   switch (prop_id) {
 282     case ARG_NUM_BANDS:
 283       g_value_set_uint (value, equ->freq_count);
 284       break;
 285     case ARG_BAND_WIDTH:
 286       g_value_set_double (value, equ->band_width);
 287       break;
 288     default:
 289       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
 290       break;
 291   }
 292   GST_OBJECT_UNLOCK (equ);
 293   GST_EQUALIZER_TRANSFORM_UNLOCK (equ);
 294 }
 295
 296 /* start of code that is type specific */
 297
 298 #define CREATE_OPTIMIZED_FUNCTIONS(TYPE,BIG_TYPE,MIN_VAL,MAX_VAL)       \
 299 typedef struct {                                                        \
 300   TYPE x1, x2;          /* history of input values for a filter */      \
 301   TYPE y1, y2;          /* history of output values for a filter */     \
 302 } SecondOrderHistory ## TYPE;                                           \
 303                                                                         \
 304 static inline TYPE                                                      \
 305 one_step_ ## TYPE (SecondOrderFilter *filter,                           \
 306     SecondOrderHistory ## TYPE *history, TYPE input)                    \
 307 {                                                                       \
 308   /* calculate output */                                                \
 309   TYPE output = filter->alpha * (input - history->x2) +                 \
 310     filter->gamma * history->y1 - filter->beta * history->y2;           \
 311   /* update history */                                                  \
 312   history->y2 = history->y1;                                            \
 313   history->y1 = output;                                                 \
 314   history->x2 = history->x1;                                            \
 315   history->x1 = input;                                                  \
 316                                                                         \
 317   return output;                                                        \
 318 }                                                                       \
 319                                                                         \
 320 static const guint                                                      \
 321 history_size_ ## TYPE = sizeof (SecondOrderHistory ## TYPE);            \
 322                                                                         \
 323 static void                                                             \
 324 gst_iir_equ_process_ ## TYPE (GstIirEqualizer *equ, guint8 *data,       \
 325 guint size, guint channels)                                             \
 326 {                                                                       \
 327   guint frames = size / channels / sizeof (TYPE);                       \
 328   guint i, c, f;                                                        \
 329   BIG_TYPE cur;                                                         \
 330   TYPE val;                                                             \
 331                                                                         \
 332   for (i = 0; i < frames; i++) {                                        \
 333     for (c = 0; c < channels; c++) {                                    \
 334       SecondOrderHistory ## TYPE *history = equ->history;               \
 335       val = *((TYPE *) data);                                           \
 336       cur = 0;                                                          \
 337       for (f = 0; f < equ->freq_count; f++) {                           \
 338         SecondOrderFilter *filter = &equ->filter[f];                    \
 339                                                                         \
 340         cur += one_step_ ## TYPE (filter, history, val);                \
 341         history++;                                                      \
 342       }                                                                 \
 343       cur += val * 0.25;                                                \
 344       cur = CLAMP (cur, MIN_VAL, MAX_VAL);                              \
 345       *((TYPE *) data) = (TYPE) cur;                                    \
 346       data += sizeof (TYPE);                                            \
 347     }                                                                   \
 348   }                                                                     \
 349 }
 350
 351 CREATE_OPTIMIZED_FUNCTIONS (gint16, gint, -32768, 32767);
 352 CREATE_OPTIMIZED_FUNCTIONS (gfloat, gfloat, -1.0, 1.0);
 353
 354 static GstFlowReturn
 355 gst_iir_equalizer_transform_ip (GstBaseTransform * btrans, GstBuffer * buf)
 356 {
 357   GstAudioFilter *filter = GST_AUDIO_FILTER (btrans);
 358   GstIirEqualizer *equ = GST_IIR_EQUALIZER (btrans);
 359
 360   if (G_UNLIKELY (filter->format.channels < 1 || equ->process == NULL))
 361     return GST_FLOW_NOT_NEGOTIATED;
 362
 363   equ->process (equ, GST_BUFFER_DATA (buf), GST_BUFFER_SIZE (buf),
 364       filter->format.channels);
 365
 366   return GST_FLOW_OK;
 367 }
 368
 369 static gboolean
 370 gst_iir_equalizer_setup (GstAudioFilter * audio, GstRingBufferSpec * fmt)
 371 {
 372   GstIirEqualizer *equ = GST_IIR_EQUALIZER (audio);
 373
 374   switch (fmt->width) {
 375     case 16:
 376       equ->history_size = history_size_gint16;
 377       equ->process = gst_iir_equ_process_gint16;
 378       break;
 379     case 32:
 380       equ->history_size = history_size_gfloat;
 381       equ->process = gst_iir_equ_process_gfloat;
 382       break;
 383     default:
 384       return FALSE;
 385   }
 386   gst_iir_equalizer_compute_frequencies (equ, equ->freq_count);
 387   return TRUE;
 388 }
 389
 390 static gboolean
 391 plugin_init (GstPlugin * plugin)
 392 {
 393   GST_DEBUG_CATEGORY_INIT (equalizer_debug, "equalizer", 0, "equalizer");
 394
 395   return gst_element_register (plugin, "equalizer", GST_RANK_NONE,
 396       GST_TYPE_IIR_EQUALIZER);
 397 }
 398
 399 GST_PLUGIN_DEFINE (GST_VERSION_MAJOR,
 400     GST_VERSION_MINOR,
 401     "equalizer",
 402     "GStreamer equalizers",
 403     plugin_init, VERSION, GST_LICENSE, GST_PACKAGE_NAME, GST_PACKAGE_ORIGIN)