gst/audiofx/audiofxbaseiirfilter.c

   1 /*
   2  * GStreamer
   3  * Copyright (C) 2007-2009 Sebastian Dröge <sebastian.droege@collabora.co.uk>
   4  *
   5  * This library is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU Library General Public
   7  * License as published by the Free Software Foundation; either
   8  * version 2 of the License, or (at your option) any later version.
   9  *
  10  * This library is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13  * Library General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU Library General Public
  16  * License along with this library; if not, write to the
  17  * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
  18  * Boston, MA 02111-1307, USA.
  19  */
  20
  21 #ifdef HAVE_CONFIG_H
  22 #include "config.h"
  23 #endif
  24
  25 #include <string.h>
  26
  27 #include <gst/gst.h>
  28 #include <gst/base/gstbasetransform.h>
  29 #include <gst/audio/audio.h>
  30 #include <gst/audio/gstaudiofilter.h>
  31
  32 #include <math.h>
  33
  34 #include "audiofxbaseiirfilter.h"
  35
  36 #define GST_CAT_DEFAULT gst_audio_fx_base_iir_filter_debug
  37 GST_DEBUG_CATEGORY_STATIC (GST_CAT_DEFAULT);
  38
  39 #define ALLOWED_CAPS \
  40     "audio/x-raw,"                                                \
  41     " format=(string){"GST_AUDIO_NE(F32)","GST_AUDIO_NE(F64)"},"  \
  42     " rate = (int) [ 1, MAX ],"                                   \
  43     " channels = (int) [ 1, MAX ],"                               \
  44     " layout=(string) interleaved"
  45
  46 #define gst_audio_fx_base_iir_filter_parent_class parent_class
  47 G_DEFINE_TYPE (GstAudioFXBaseIIRFilter,
  48     gst_audio_fx_base_iir_filter, GST_TYPE_AUDIO_FILTER);
  49
  50 static gboolean gst_audio_fx_base_iir_filter_setup (GstAudioFilter * filter,
  51     const GstAudioInfo * info);
  52 static GstFlowReturn
  53 gst_audio_fx_base_iir_filter_transform_ip (GstBaseTransform * base,
  54     GstBuffer * buf);
  55 static gboolean gst_audio_fx_base_iir_filter_stop (GstBaseTransform * base);
  56
  57 static void process_64 (GstAudioFXBaseIIRFilter * filter,
  58     gdouble * data, guint num_samples);
  59 static void process_32 (GstAudioFXBaseIIRFilter * filter,
  60     gfloat * data, guint num_samples);
  61
  62 /* GObject vmethod implementations */
  63
  64 static void
  65 gst_audio_fx_base_iir_filter_finalize (GObject * object)
  66 {
  67   GstAudioFXBaseIIRFilter *filter = GST_AUDIO_FX_BASE_IIR_FILTER (object);
  68
  69   if (filter->a) {
  70     g_free (filter->a);
  71     filter->a = NULL;
  72   }
  73
  74   if (filter->b) {
  75     g_free (filter->b);
  76     filter->b = NULL;
  77   }
  78
  79   if (filter->channels) {
  80     GstAudioFXBaseIIRFilterChannelCtx *ctx;
  81     guint i;
  82
  83     for (i = 0; i < filter->nchannels; i++) {
  84       ctx = &filter->channels[i];
  85       g_free (ctx->x);
  86       g_free (ctx->y);
  87     }
  88
  89     g_free (filter->channels);
  90     filter->channels = NULL;
  91   }
  92   g_mutex_clear (&filter->lock);
  93
  94   G_OBJECT_CLASS (parent_class)->finalize (object);
  95 }
  96
  97 static void
  98 gst_audio_fx_base_iir_filter_class_init (GstAudioFXBaseIIRFilterClass * klass)
  99 {
 100   GObjectClass *gobject_class = (GObjectClass *) klass;
 101   GstBaseTransformClass *trans_class = (GstBaseTransformClass *) klass;
 102   GstAudioFilterClass *filter_class = (GstAudioFilterClass *) klass;
 103   GstCaps *caps;
 104
 105   GST_DEBUG_CATEGORY_INIT (gst_audio_fx_base_iir_filter_debug,
 106       "audiofxbaseiirfilter", 0, "Audio IIR Filter Base Class");
 107
 108   gobject_class->finalize = gst_audio_fx_base_iir_filter_finalize;
 109
 110   caps = gst_caps_from_string (ALLOWED_CAPS);
 111   gst_audio_filter_class_add_pad_templates (GST_AUDIO_FILTER_CLASS (klass),
 112       caps);
 113   gst_caps_unref (caps);
 114
 115   filter_class->setup = GST_DEBUG_FUNCPTR (gst_audio_fx_base_iir_filter_setup);
 116
 117   trans_class->transform_ip =
 118       GST_DEBUG_FUNCPTR (gst_audio_fx_base_iir_filter_transform_ip);
 119   trans_class->stop = GST_DEBUG_FUNCPTR (gst_audio_fx_base_iir_filter_stop);
 120 }
 121
 122 static void
 123 gst_audio_fx_base_iir_filter_init (GstAudioFXBaseIIRFilter * filter)
 124 {
 125   gst_base_transform_set_in_place (GST_BASE_TRANSFORM (filter), TRUE);
 126
 127   filter->a = NULL;
 128   filter->na = 0;
 129   filter->b = NULL;
 130   filter->nb = 0;
 131   filter->channels = NULL;
 132   filter->nchannels = 0;
 133
 134   g_mutex_init (&filter->lock);
 135 }
 136
 137 /* Evaluate the transfer function that corresponds to the IIR
 138  * coefficients at (zr + zi*I)^-1 and return the magnitude */
 139 gdouble
 140 gst_audio_fx_base_iir_filter_calculate_gain (gdouble * a, guint na, gdouble * b,
 141     guint nb, gdouble zr, gdouble zi)
 142 {
 143   gdouble sum_ar, sum_ai;
 144   gdouble sum_br, sum_bi;
 145   gdouble gain_r, gain_i;
 146
 147   gdouble sum_r_old;
 148   gdouble sum_i_old;
 149
 150   gint i;
 151
 152   sum_ar = a[na - 1];
 153   sum_ai = 0.0;
 154   for (i = na - 2; i >= 0; i--) {
 155     sum_r_old = sum_ar;
 156     sum_i_old = sum_ai;
 157
 158     sum_ar = (sum_r_old * zr - sum_i_old * zi) + a[i];
 159     sum_ai = (sum_r_old * zi + sum_i_old * zr) + 0.0;
 160   }
 161
 162   sum_br = b[nb - 1];
 163   sum_bi = 0.0;
 164   for (i = nb - 2; i >= 0; i--) {
 165     sum_r_old = sum_br;
 166     sum_i_old = sum_bi;
 167
 168     sum_br = (sum_r_old * zr - sum_i_old * zi) + b[i];
 169     sum_bi = (sum_r_old * zi + sum_i_old * zr) + 0.0;
 170   }
 171
 172   gain_r =
 173       (sum_br * sum_ar + sum_bi * sum_ai) / (sum_ar * sum_ar + sum_ai * sum_ai);
 174   gain_i =
 175       (sum_bi * sum_ar - sum_br * sum_ai) / (sum_ar * sum_ar + sum_ai * sum_ai);
 176
 177   return (sqrt (gain_r * gain_r + gain_i * gain_i));
 178 }
 179
 180 void
 181 gst_audio_fx_base_iir_filter_set_coefficients (GstAudioFXBaseIIRFilter * filter,
 182     gdouble * a, guint na, gdouble * b, guint nb)
 183 {
 184   guint i;
 185
 186   g_return_if_fail (GST_IS_AUDIO_FX_BASE_IIR_FILTER (filter));
 187
 188   g_mutex_lock (&filter->lock);
 189
 190   g_free (filter->a);
 191   g_free (filter->b);
 192
 193   filter->a = filter->b = NULL;
 194
 195   if (filter->channels) {
 196     GstAudioFXBaseIIRFilterChannelCtx *ctx;
 197     gboolean free = (na != filter->na || nb != filter->nb);
 198
 199     for (i = 0; i < filter->nchannels; i++) {
 200       ctx = &filter->channels[i];
 201
 202       if (free)
 203         g_free (ctx->x);
 204       else
 205         memset (ctx->x, 0, filter->nb * sizeof (gdouble));
 206
 207       if (free)
 208         g_free (ctx->y);
 209       else
 210         memset (ctx->y, 0, filter->na * sizeof (gdouble));
 211     }
 212
 213     g_free (filter->channels);
 214     filter->channels = NULL;
 215   }
 216
 217   filter->na = na;
 218   filter->nb = nb;
 219
 220   filter->a = a;
 221   filter->b = b;
 222
 223   if (filter->nchannels && !filter->channels) {
 224     GstAudioFXBaseIIRFilterChannelCtx *ctx;
 225
 226     filter->channels =
 227         g_new0 (GstAudioFXBaseIIRFilterChannelCtx, filter->nchannels);
 228     for (i = 0; i < filter->nchannels; i++) {
 229       ctx = &filter->channels[i];
 230
 231       ctx->x = g_new0 (gdouble, filter->nb);
 232       ctx->y = g_new0 (gdouble, filter->na);
 233     }
 234   }
 235
 236   g_mutex_unlock (&filter->lock);
 237 }
 238
 239 /* GstAudioFilter vmethod implementations */
 240
 241 static gboolean
 242 gst_audio_fx_base_iir_filter_setup (GstAudioFilter * base,
 243     const GstAudioInfo * info)
 244 {
 245   GstAudioFXBaseIIRFilter *filter = GST_AUDIO_FX_BASE_IIR_FILTER (base);
 246   gboolean ret = TRUE;
 247   gint channels;
 248
 249   g_mutex_lock (&filter->lock);
 250   switch (GST_AUDIO_INFO_FORMAT (info)) {
 251     case GST_AUDIO_FORMAT_F32:
 252       filter->process = (GstAudioFXBaseIIRFilterProcessFunc)
 253           process_32;
 254       break;
 255     case GST_AUDIO_FORMAT_F64:
 256       filter->process = (GstAudioFXBaseIIRFilterProcessFunc)
 257           process_64;
 258       break;
 259     default:
 260       ret = FALSE;
 261       break;
 262   }
 263
 264   channels = GST_AUDIO_INFO_CHANNELS (info);
 265
 266   if (channels != filter->nchannels) {
 267     guint i;
 268     GstAudioFXBaseIIRFilterChannelCtx *ctx;
 269
 270     if (filter->channels) {
 271       for (i = 0; i < filter->nchannels; i++) {
 272         ctx = &filter->channels[i];
 273
 274         g_free (ctx->x);
 275         g_free (ctx->y);
 276       }
 277       g_free (filter->channels);
 278     }
 279
 280     filter->channels = g_new0 (GstAudioFXBaseIIRFilterChannelCtx, channels);
 281     for (i = 0; i < channels; i++) {
 282       ctx = &filter->channels[i];
 283
 284       ctx->x = g_new0 (gdouble, filter->nb);
 285       ctx->y = g_new0 (gdouble, filter->na);
 286     }
 287     filter->nchannels = channels;
 288   }
 289   g_mutex_unlock (&filter->lock);
 290
 291   return ret;
 292 }
 293
 294 static inline gdouble
 295 process (GstAudioFXBaseIIRFilter * filter,
 296     GstAudioFXBaseIIRFilterChannelCtx * ctx, gdouble x0)
 297 {
 298   gdouble val = filter->b[0] * x0;
 299   gint i, j;
 300
 301   for (i = 1, j = ctx->x_pos; i < filter->nb; i++) {
 302     val += filter->b[i] * ctx->x[j];
 303     j--;
 304     if (j < 0)
 305       j = filter->nb - 1;
 306   }
 307
 308   for (i = 1, j = ctx->y_pos; i < filter->na; i++) {
 309     val -= filter->a[i] * ctx->y[j];
 310     j--;
 311     if (j < 0)
 312       j = filter->na - 1;
 313   }
 314   val /= filter->a[0];
 315
 316   if (ctx->x) {
 317     ctx->x_pos++;
 318     if (ctx->x_pos >= filter->nb)
 319       ctx->x_pos = 0;
 320     ctx->x[ctx->x_pos] = x0;
 321   }
 322   if (ctx->y) {
 323     ctx->y_pos++;
 324     if (ctx->y_pos >= filter->na)
 325       ctx->y_pos = 0;
 326
 327     ctx->y[ctx->y_pos] = val;
 328   }
 329
 330   return val;
 331 }
 332
 333 #define DEFINE_PROCESS_FUNC(width,ctype) \
 334 static void \
 335 process_##width (GstAudioFXBaseIIRFilter * filter, \
 336     g##ctype * data, guint num_samples) \
 337 { \
 338   gint i, j, channels = filter->nchannels; \
 339   gdouble val; \
 340   \
 341   for (i = 0; i < num_samples / channels; i++) { \
 342     for (j = 0; j < channels; j++) { \
 343       val = process (filter, &filter->channels[j], *data); \
 344       *data++ = val; \
 345     } \
 346   } \
 347 }
 348
 349 DEFINE_PROCESS_FUNC (32, float);
 350 DEFINE_PROCESS_FUNC (64, double);
 351
 352 #undef DEFINE_PROCESS_FUNC
 353
 354 /* GstBaseTransform vmethod implementations */
 355 static GstFlowReturn
 356 gst_audio_fx_base_iir_filter_transform_ip (GstBaseTransform * base,
 357     GstBuffer * buf)
 358 {
 359   GstAudioFXBaseIIRFilter *filter = GST_AUDIO_FX_BASE_IIR_FILTER (base);
 360   guint num_samples;
 361   GstClockTime timestamp, stream_time;
 362   GstMapInfo map;
 363
 364   timestamp = GST_BUFFER_TIMESTAMP (buf);
 365   stream_time =
 366       gst_segment_to_stream_time (&base->segment, GST_FORMAT_TIME, timestamp);
 367
 368   GST_DEBUG_OBJECT (filter, "sync to %" GST_TIME_FORMAT,
 369       GST_TIME_ARGS (timestamp));
 370
 371   if (GST_CLOCK_TIME_IS_VALID (stream_time))
 372     gst_object_sync_values (GST_OBJECT (filter), stream_time);
 373
 374   if (gst_base_transform_is_passthrough (base))
 375     return GST_FLOW_OK;
 376
 377   g_return_val_if_fail (filter->a != NULL, GST_FLOW_ERROR);
 378
 379   gst_buffer_map (buf, &map, GST_MAP_READWRITE);
 380   num_samples = map.size / GST_AUDIO_FILTER_BPS (filter);
 381
 382   g_mutex_lock (&filter->lock);
 383   filter->process (filter, map.data, num_samples);
 384   g_mutex_unlock (&filter->lock);
 385
 386   gst_buffer_unmap (buf, &map);
 387
 388   return GST_FLOW_OK;
 389 }
 390
 391
 392 static gboolean
 393 gst_audio_fx_base_iir_filter_stop (GstBaseTransform * base)
 394 {
 395   GstAudioFXBaseIIRFilter *filter = GST_AUDIO_FX_BASE_IIR_FILTER (base);
 396   guint channels = filter->nchannels;
 397   GstAudioFXBaseIIRFilterChannelCtx *ctx;
 398   guint i;
 399
 400   /* Reset the history of input and output values if
 401    * already existing */
 402   if (channels && filter->channels) {
 403     for (i = 0; i < channels; i++) {
 404       ctx = &filter->channels[i];
 405       g_free (ctx->x);
 406       g_free (ctx->y);
 407     }
 408     g_free (filter->channels);
 409   }
 410   filter->channels = NULL;
 411   filter->nchannels = 0;
 412
 413   return TRUE;
 414 }