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