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       GST_BUFFER_SIZE (buf) / (GST_AUDIO_FILTER (filter)->format.width / 8);

   360 

   361   if (GST_CLOCK_TIME_IS_VALID (GST_BUFFER_TIMESTAMP (buf)))

   362     gst_object_sync_values (G_OBJECT (filter), GST_BUFFER_TIMESTAMP (buf));

   363 

   364   if (gst_base_transform_is_passthrough (base))

   365     return GST_FLOW_OK;

   366 

   367   g_return_val_if_fail (filter->a != NULL, GST_FLOW_ERROR);

   368 

   369   filter->process (filter, GST_BUFFER_DATA (buf), num_samples);

   370 

   371   return GST_FLOW_OK;

   372 }

   373 

   374 

   375 static gboolean

   376 gst_audio_fx_base_iir_filter_stop (GstBaseTransform * base)

   377 {

   378   GstAudioFXBaseIIRFilter *filter = GST_AUDIO_FX_BASE_IIR_FILTER (base);

   379   guint channels = GST_AUDIO_FILTER (filter)->format.channels;

   380   GstAudioFXBaseIIRFilterChannelCtx *ctx;

   381   guint i;

   382 

   383   /* Reset the history of input and output values if

   384    * already existing */

   385   if (channels && filter->channels) {

   386     for (i = 0; i < channels; i++) {

   387       ctx = &filter->channels[i];

   388       g_free (ctx->x);

   389       g_free (ctx->y);

   390     }

   391     g_free (filter->channels);

   392   }

   393   filter->channels = NULL;

   394 

   395   return TRUE;

   396 }