diff options
Diffstat (limited to 'gst/audiofx/audiofxbaseiirfilter.c')
-rw-r--r-- | gst/audiofx/audiofxbaseiirfilter.c | 396 |
1 files changed, 396 insertions, 0 deletions
diff --git a/gst/audiofx/audiofxbaseiirfilter.c b/gst/audiofx/audiofxbaseiirfilter.c new file mode 100644 index 00000000..29cb2440 --- /dev/null +++ b/gst/audiofx/audiofxbaseiirfilter.c @@ -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, "audiobaseiirfilter", 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; +} |