"Fossies" - the Fresh Open Source Software Archive

Member "gst-plugins-good-1.20.3/gst/audiofx/audiofxbaseiirfilter.c" (15 Jun 2022, 10990 Bytes) of package /linux/misc/gst-plugins-good-1.20.3.tar.xz:


As a special service "Fossies" has tried to format the requested source page into HTML format using (guessed) C and C++ source code syntax highlighting (style: standard) with prefixed line numbers and code folding option. Alternatively you can here view or download the uninterpreted source code file. For more information about "audiofxbaseiirfilter.c" see the Fossies "Dox" file reference documentation.

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