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_dispose (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
  93   G_OBJECT_CLASS (parent_class)->dispose (object);
  94 }
  95
  96 static void
  97 gst_audio_fx_base_iir_filter_class_init (GstAudioFXBaseIIRFilterClass * klass)
  98 {
  99   GObjectClass *gobject_class = (GObjectClass *) klass;
 100   GstBaseTransformClass *trans_class = (GstBaseTransformClass *) klass;
 101   GstAudioFilterClass *filter_class = (GstAudioFilterClass *) klass;
 102   GstCaps *caps;
 103
 104   GST_DEBUG_CATEGORY_INIT (gst_audio_fx_base_iir_filter_debug,
 105       "audiofxbaseiirfilter", 0, "Audio IIR Filter Base Class");
 106
 107   gobject_class->dispose = gst_audio_fx_base_iir_filter_dispose;
 108
 109   caps = gst_caps_from_string (ALLOWED_CAPS);
 110   gst_audio_filter_class_add_pad_templates (GST_AUDIO_FILTER_CLASS (klass),
 111       caps);
 112   gst_caps_unref (caps);
 113
 114   filter_class->setup = GST_DEBUG_FUNCPTR (gst_audio_fx_base_iir_filter_setup);
 115
 116   trans_class->transform_ip =
 117       GST_DEBUG_FUNCPTR (gst_audio_fx_base_iir_filter_transform_ip);
 118   trans_class->stop = GST_DEBUG_FUNCPTR (gst_audio_fx_base_iir_filter_stop);
 119 }
 120
 121 static void
 122 gst_audio_fx_base_iir_filter_init (GstAudioFXBaseIIRFilter * filter)
 123 {
 124   gst_base_transform_set_in_place (GST_BASE_TRANSFORM (filter), TRUE);
 125
 126   filter->a = NULL;
 127   filter->na = 0;
 128   filter->b = NULL;
 129   filter->nb = 0;
 130   filter->channels = NULL;
 131   filter->nchannels = 0;
 132 }
 133
 134 /* Evaluate the transfer function that corresponds to the IIR
 135  * coefficients at (zr + zi*I)^-1 and return the magnitude */
 136 gdouble
 137 gst_audio_fx_base_iir_filter_calculate_gain (gdouble * a, guint na, gdouble * b,
 138     guint nb, gdouble zr, gdouble zi)
 139 {
 140   gdouble sum_ar, sum_ai;
 141   gdouble sum_br, sum_bi;
 142   gdouble gain_r, gain_i;
 143
 144   gdouble sum_r_old;
 145   gdouble sum_i_old;
 146
 147   gint i;
 148
 149   sum_ar = a[na - 1];
 150   sum_ai = 0.0;
 151   for (i = na - 2; i >= 0; i--) {
 152     sum_r_old = sum_ar;
 153     sum_i_old = sum_ai;
 154
 155     sum_ar = (sum_r_old * zr - sum_i_old * zi) + a[i];
 156     sum_ai = (sum_r_old * zi + sum_i_old * zr) + 0.0;
 157   }
 158
 159   sum_br = b[nb - 1];
 160   sum_bi = 0.0;
 161   for (i = nb - 2; i >= 0; i--) {
 162     sum_r_old = sum_br;
 163     sum_i_old = sum_bi;
 164
 165     sum_br = (sum_r_old * zr - sum_i_old * zi) + b[i];
 166     sum_bi = (sum_r_old * zi + sum_i_old * zr) + 0.0;
 167   }
 168
 169   gain_r =
 170       (sum_br * sum_ar + sum_bi * sum_ai) / (sum_ar * sum_ar + sum_ai * sum_ai);
 171   gain_i =
 172       (sum_bi * sum_ar - sum_br * sum_ai) / (sum_ar * sum_ar + sum_ai * sum_ai);
 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->nb * sizeof (gdouble));
 203
 204       if (free)
 205         g_free (ctx->y);
 206       else
 207         memset (ctx->y, 0, filter->na * 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->nb);
 229       ctx->y = g_new0 (gdouble, filter->na);
 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 < channels; i++) {
 278       ctx = &filter->channels[i];
 279
 280       ctx->x = g_new0 (gdouble, filter->nb);
 281       ctx->y = g_new0 (gdouble, filter->na);
 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->b[0] * x0;
 294   gint i, j;
 295
 296   for (i = 1, j = ctx->x_pos; i < filter->nb; i++) {
 297     val += filter->b[i] * ctx->x[j];
 298     j--;
 299     if (j < 0)
 300       j = filter->nb - 1;
 301   }
 302
 303   for (i = 1, j = ctx->y_pos; i < filter->na; i++) {
 304     val -= filter->a[i] * ctx->y[j];
 305     j--;
 306     if (j < 0)
 307       j = filter->na - 1;
 308   }
 309   val /= filter->a[0];
 310
 311   if (ctx->x) {
 312     ctx->x_pos++;
 313     if (ctx->x_pos >= filter->nb)
 314       ctx->x_pos = 0;
 315     ctx->x[ctx->x_pos] = x0;
 316   }
 317   if (ctx->y) {
 318     ctx->y_pos++;
 319     if (ctx->y_pos >= filter->na)
 320       ctx->y_pos = 0;
 321
 322     ctx->y[ctx->y_pos] = val;
 323   }
 324
 325   return val;
 326 }
 327
 328 #define DEFINE_PROCESS_FUNC(width,ctype) \
 329 static void \
 330 process_##width (GstAudioFXBaseIIRFilter * filter, \
 331     g##ctype * data, guint num_samples) \
 332 { \
 333   gint i, j, channels = filter->nchannels; \
 334   gdouble val; \
 335   \
 336   for (i = 0; i < num_samples / channels; i++) { \
 337     for (j = 0; j < channels; j++) { \
 338       val = process (filter, &filter->channels[j], *data); \
 339       *data++ = val; \
 340     } \
 341   } \
 342 }
 343
 344 DEFINE_PROCESS_FUNC (32, float);
 345 DEFINE_PROCESS_FUNC (64, double);
 346
 347 #undef DEFINE_PROCESS_FUNC
 348
 349 /* GstBaseTransform vmethod implementations */
 350 static GstFlowReturn
 351 gst_audio_fx_base_iir_filter_transform_ip (GstBaseTransform * base,
 352     GstBuffer * buf)
 353 {
 354   GstAudioFXBaseIIRFilter *filter = GST_AUDIO_FX_BASE_IIR_FILTER (base);
 355   guint num_samples;
 356   GstClockTime timestamp, stream_time;
 357   GstMapInfo map;
 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 (GST_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   gst_buffer_map (buf, &map, GST_MAP_READWRITE);
 375   num_samples = map.size / GST_AUDIO_FILTER_BPS (filter);
 376
 377   filter->process (filter, map.data, num_samples);
 378
 379   gst_buffer_unmap (buf, &map);
 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 }