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