--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gst_plugins_good/gst/audiofx/audiofxbaseiirfilter.c	Fri Apr 16 15:15:52 2010 +0300
@@ -0,0 +1,396 @@
+/* 
+ * GStreamer
+ * Copyright (C) 2007-2009 Sebastian Dröge <sebastian.droege@collabora.co.uk>
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Library General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU Library General Public
+ * License along with this library; if not, write to the
+ * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <gst/gst.h>
+#include <gst/base/gstbasetransform.h>
+#include <gst/audio/audio.h>
+#include <gst/audio/gstaudiofilter.h>
+#include <gst/controller/gstcontroller.h>
+
+#include <math.h>
+
+#include "audiofxbaseiirfilter.h"
+
+#define GST_CAT_DEFAULT gst_audio_fx_base_iir_filter_debug
+GST_DEBUG_CATEGORY_STATIC (GST_CAT_DEFAULT);
+
+#define ALLOWED_CAPS \
+    "audio/x-raw-float,"                                              \
+    " width = (int) { 32, 64 }, "                                     \
+    " endianness = (int) BYTE_ORDER,"                                 \
+    " rate = (int) [ 1, MAX ],"                                       \
+    " channels = (int) [ 1, MAX ]"
+
+#define DEBUG_INIT(bla) \
+  GST_DEBUG_CATEGORY_INIT (gst_audio_fx_base_iir_filter_debug, "audiofxbaseiirfilter", 0, "Audio IIR Filter Base Class");
+
+GST_BOILERPLATE_FULL (GstAudioFXBaseIIRFilter,
+    gst_audio_fx_base_iir_filter, GstAudioFilter, GST_TYPE_AUDIO_FILTER,
+    DEBUG_INIT);
+
+static gboolean gst_audio_fx_base_iir_filter_setup (GstAudioFilter * filter,
+    GstRingBufferSpec * format);
+static GstFlowReturn
+gst_audio_fx_base_iir_filter_transform_ip (GstBaseTransform * base,
+    GstBuffer * buf);
+static gboolean gst_audio_fx_base_iir_filter_stop (GstBaseTransform * base);
+
+static void process_64 (GstAudioFXBaseIIRFilter * filter,
+    gdouble * data, guint num_samples);
+static void process_32 (GstAudioFXBaseIIRFilter * filter,
+    gfloat * data, guint num_samples);
+
+/* GObject vmethod implementations */
+
+static void
+gst_audio_fx_base_iir_filter_base_init (gpointer klass)
+{
+  GstCaps *caps;
+
+  caps = gst_caps_from_string (ALLOWED_CAPS);
+  gst_audio_filter_class_add_pad_templates (GST_AUDIO_FILTER_CLASS (klass),
+      caps);
+  gst_caps_unref (caps);
+}
+
+static void
+gst_audio_fx_base_iir_filter_dispose (GObject * object)
+{
+  GstAudioFXBaseIIRFilter *filter = GST_AUDIO_FX_BASE_IIR_FILTER (object);
+
+  if (filter->a) {
+    g_free (filter->a);
+    filter->a = NULL;
+  }
+
+  if (filter->b) {
+    g_free (filter->b);
+    filter->b = NULL;
+  }
+
+  if (filter->channels) {
+    GstAudioFXBaseIIRFilterChannelCtx *ctx;
+    guint i;
+
+    for (i = 0; i < filter->nchannels; i++) {
+      ctx = &filter->channels[i];
+      g_free (ctx->x);
+      g_free (ctx->y);
+    }
+
+    g_free (filter->channels);
+    filter->channels = NULL;
+  }
+
+  G_OBJECT_CLASS (parent_class)->dispose (object);
+}
+
+static void
+gst_audio_fx_base_iir_filter_class_init (GstAudioFXBaseIIRFilterClass * klass)
+{
+  GObjectClass *gobject_class = (GObjectClass *) klass;
+  GstBaseTransformClass *trans_class = (GstBaseTransformClass *) klass;
+  GstAudioFilterClass *filter_class = (GstAudioFilterClass *) klass;
+
+  gobject_class->dispose = gst_audio_fx_base_iir_filter_dispose;
+
+  filter_class->setup = GST_DEBUG_FUNCPTR (gst_audio_fx_base_iir_filter_setup);
+
+  trans_class->transform_ip =
+      GST_DEBUG_FUNCPTR (gst_audio_fx_base_iir_filter_transform_ip);
+  trans_class->stop = GST_DEBUG_FUNCPTR (gst_audio_fx_base_iir_filter_stop);
+}
+
+static void
+gst_audio_fx_base_iir_filter_init (GstAudioFXBaseIIRFilter * filter,
+    GstAudioFXBaseIIRFilterClass * klass)
+{
+  gst_base_transform_set_in_place (GST_BASE_TRANSFORM (filter), TRUE);
+
+  filter->a = NULL;
+  filter->na = 0;
+  filter->b = NULL;
+  filter->nb = 0;
+  filter->channels = NULL;
+  filter->nchannels = 0;
+}
+
+/* Evaluate the transfer function that corresponds to the IIR
+ * coefficients at zr + zi*I and return the magnitude */
+gdouble
+gst_audio_fx_base_iir_filter_calculate_gain (gdouble * a, guint na, gdouble * b,
+    guint nb, gdouble zr, gdouble zi)
+{
+  gdouble sum_ar, sum_ai;
+  gdouble sum_br, sum_bi;
+  gdouble gain_r, gain_i;
+
+  gdouble sum_r_old;
+  gdouble sum_i_old;
+
+  gint i;
+
+  sum_ar = 0.0;
+  sum_ai = 0.0;
+  for (i = na - 1; i >= 0; i--) {
+    sum_r_old = sum_ar;
+    sum_i_old = sum_ai;
+
+    sum_ar = (sum_r_old * zr - sum_i_old * zi) + a[i];
+    sum_ai = (sum_r_old * zi + sum_i_old * zr) + 0.0;
+  }
+
+  sum_br = 0.0;
+  sum_bi = 0.0;
+  for (i = nb - 1; i >= 0; i--) {
+    sum_r_old = sum_br;
+    sum_i_old = sum_bi;
+
+    sum_br = (sum_r_old * zr - sum_i_old * zi) - b[i];
+    sum_bi = (sum_r_old * zi + sum_i_old * zr) - 0.0;
+  }
+  sum_br += 1.0;
+  sum_bi += 0.0;
+
+  gain_r =
+      (sum_ar * sum_br + sum_ai * sum_bi) / (sum_br * sum_br + sum_bi * sum_bi);
+  gain_i =
+      (sum_ai * sum_br - sum_ar * sum_bi) / (sum_br * sum_br + sum_bi * sum_bi);
+
+  return (sqrt (gain_r * gain_r + gain_i * gain_i));
+}
+
+void
+gst_audio_fx_base_iir_filter_set_coefficients (GstAudioFXBaseIIRFilter * filter,
+    gdouble * a, guint na, gdouble * b, guint nb)
+{
+  guint i;
+
+  g_return_if_fail (GST_IS_AUDIO_FX_BASE_IIR_FILTER (filter));
+
+  GST_BASE_TRANSFORM_LOCK (filter);
+
+  g_free (filter->a);
+  g_free (filter->b);
+
+  filter->a = filter->b = NULL;
+
+  if (filter->channels) {
+    GstAudioFXBaseIIRFilterChannelCtx *ctx;
+    gboolean free = (na != filter->na || nb != filter->nb);
+
+    for (i = 0; i < filter->nchannels; i++) {
+      ctx = &filter->channels[i];
+
+      if (free)
+        g_free (ctx->x);
+      else
+        memset (ctx->x, 0, filter->na * sizeof (gdouble));
+
+      if (free)
+        g_free (ctx->y);
+      else
+        memset (ctx->y, 0, filter->nb * sizeof (gdouble));
+    }
+
+    g_free (filter->channels);
+    filter->channels = NULL;
+  }
+
+  filter->na = na;
+  filter->nb = nb;
+
+  filter->a = a;
+  filter->b = b;
+
+  if (filter->nchannels && !filter->channels) {
+    GstAudioFXBaseIIRFilterChannelCtx *ctx;
+
+    filter->channels =
+        g_new0 (GstAudioFXBaseIIRFilterChannelCtx, filter->nchannels);
+    for (i = 0; i < filter->nchannels; i++) {
+      ctx = &filter->channels[i];
+
+      ctx->x = g_new0 (gdouble, filter->na);
+      ctx->y = g_new0 (gdouble, filter->nb);
+    }
+  }
+
+  GST_BASE_TRANSFORM_UNLOCK (filter);
+}
+
+/* GstAudioFilter vmethod implementations */
+
+static gboolean
+gst_audio_fx_base_iir_filter_setup (GstAudioFilter * base,
+    GstRingBufferSpec * format)
+{
+  GstAudioFXBaseIIRFilter *filter = GST_AUDIO_FX_BASE_IIR_FILTER (base);
+  gboolean ret = TRUE;
+
+  if (format->width == 32)
+    filter->process = (GstAudioFXBaseIIRFilterProcessFunc)
+        process_32;
+  else if (format->width == 64)
+    filter->process = (GstAudioFXBaseIIRFilterProcessFunc)
+        process_64;
+  else
+    ret = FALSE;
+
+  if (format->channels != filter->nchannels) {
+    guint i;
+    GstAudioFXBaseIIRFilterChannelCtx *ctx;
+
+    if (filter->channels) {
+
+      for (i = 0; i < filter->nchannels; i++) {
+        ctx = &filter->channels[i];
+
+        g_free (ctx->x);
+        g_free (ctx->y);
+      }
+
+      g_free (filter->channels);
+      filter->channels = NULL;
+    }
+
+    filter->nchannels = format->channels;
+
+    filter->channels =
+        g_new0 (GstAudioFXBaseIIRFilterChannelCtx, filter->nchannels);
+    for (i = 0; i < filter->nchannels; i++) {
+      ctx = &filter->channels[i];
+
+      ctx->x = g_new0 (gdouble, filter->na);
+      ctx->y = g_new0 (gdouble, filter->nb);
+    }
+  }
+
+  return ret;
+}
+
+static inline gdouble
+process (GstAudioFXBaseIIRFilter * filter,
+    GstAudioFXBaseIIRFilterChannelCtx * ctx, gdouble x0)
+{
+  gdouble val = filter->a[0] * x0;
+  gint i, j;
+
+  for (i = 1, j = ctx->x_pos; i < filter->na; i++) {
+    val += filter->a[i] * ctx->x[j];
+    j--;
+    if (j < 0)
+      j = filter->na - 1;
+  }
+
+  for (i = 1, j = ctx->y_pos; i < filter->nb; i++) {
+    val += filter->b[i] * ctx->y[j];
+    j--;
+    if (j < 0)
+      j = filter->nb - 1;
+  }
+
+  if (ctx->x) {
+    ctx->x_pos++;
+    if (ctx->x_pos >= filter->na)
+      ctx->x_pos = 0;
+    ctx->x[ctx->x_pos] = x0;
+  }
+  if (ctx->y) {
+    ctx->y_pos++;
+    if (ctx->y_pos >= filter->nb)
+      ctx->y_pos = 0;
+
+    ctx->y[ctx->y_pos] = val;
+  }
+
+  return val;
+}
+
+#define DEFINE_PROCESS_FUNC(width,ctype) \
+static void \
+process_##width (GstAudioFXBaseIIRFilter * filter, \
+    g##ctype * data, guint num_samples) \
+{ \
+  gint i, j, channels = GST_AUDIO_FILTER (filter)->format.channels; \
+  gdouble val; \
+  \
+  for (i = 0; i < num_samples / channels; i++) { \
+    for (j = 0; j < channels; j++) { \
+      val = process (filter, &filter->channels[j], *data); \
+      *data++ = val; \
+    } \
+  } \
+}
+
+DEFINE_PROCESS_FUNC (32, float);
+DEFINE_PROCESS_FUNC (64, double);
+
+#undef DEFINE_PROCESS_FUNC
+
+/* GstBaseTransform vmethod implementations */
+static GstFlowReturn
+gst_audio_fx_base_iir_filter_transform_ip (GstBaseTransform * base,
+    GstBuffer * buf)
+{
+  GstAudioFXBaseIIRFilter *filter = GST_AUDIO_FX_BASE_IIR_FILTER (base);
+  guint num_samples =
+      GST_BUFFER_SIZE (buf) / (GST_AUDIO_FILTER (filter)->format.width / 8);
+
+  if (GST_CLOCK_TIME_IS_VALID (GST_BUFFER_TIMESTAMP (buf)))
+    gst_object_sync_values (G_OBJECT (filter), GST_BUFFER_TIMESTAMP (buf));
+
+  if (gst_base_transform_is_passthrough (base))
+    return GST_FLOW_OK;
+
+  g_return_val_if_fail (filter->a != NULL, GST_FLOW_ERROR);
+
+  filter->process (filter, GST_BUFFER_DATA (buf), num_samples);
+
+  return GST_FLOW_OK;
+}
+
+
+static gboolean
+gst_audio_fx_base_iir_filter_stop (GstBaseTransform * base)
+{
+  GstAudioFXBaseIIRFilter *filter = GST_AUDIO_FX_BASE_IIR_FILTER (base);
+  guint channels = GST_AUDIO_FILTER (filter)->format.channels;
+  GstAudioFXBaseIIRFilterChannelCtx *ctx;
+  guint i;
+
+  /* Reset the history of input and output values if
+   * already existing */
+  if (channels && filter->channels) {
+    for (i = 0; i < channels; i++) {
+      ctx = &filter->channels[i];
+      g_free (ctx->x);
+      g_free (ctx->y);
+    }
+    g_free (filter->channels);
+  }
+  filter->channels = NULL;
+
+  return TRUE;
+}