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