gst/audiofx/audiowsincband.c

   1 /* -*- c-basic-offset: 2 -*-
   2  * GStreamer
   3  * Copyright (C) 1999-2001 Erik Walthinsen <omega@cse.ogi.edu>
   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 /* this windowed sinc filter is taken from the freely downloadable DSP book,
  22  * "The Scientist and Engineer's Guide to Digital Signal Processing",
  23  * chapter 16
  24  * available at http://www.dspguide.com/
  25  */
  26
  27 /* FIXME:
  28  * - this filter is totally unoptimized !
  29  * - we do not destroy the allocated memory for filters and residue
  30  * - this might be improved upon with bytestream
  31  */
  32
  33 #ifdef HAVE_CONFIG_H
  34 #include "config.h"
  35 #endif
  36 #include <gst/gst.h>
  37 #include "gstfilter.h"
  38 #include <math.h>               /* M_PI */
  39 #include <string.h>             /* memmove */
  40
  41 /* elementfactory information */
  42 static GstElementDetails gst_bpwsinc_details = GST_ELEMENT_DETAILS ("BPWSinc",
  43     "Filter/Effect/Audio",
  44     "Band-Pass Windowed sinc filter",
  45     "Thomas <thomas@apestaart.org>, " "Steven W. Smith");
  46
  47 enum
  48 {
  49   /* FILL ME */
  50   LAST_SIGNAL
  51 };
  52
  53 enum
  54 {
  55   ARG_0,
  56   ARG_LENGTH,
  57   ARG_LOWER_FREQUENCY,
  58   ARG_UPPER_FREQUENCY,
  59 };
  60
  61 #define GST_TYPE_BPWSINC \
  62   (gst_bpwsinc_get_type())
  63 #define GST_BPWSINC(obj) \
  64       (G_TYPE_CHECK_INSTANCE_CAST((obj),GST_TYPE_BPWSINC,GstBPWSinc))
  65 #define GST_BPWSINC_CLASS(klass) \
  66       (G_TYPE_CHECK_CLASS_CAST((klass),GST_TYPE_ULAW,GstBPWSinc))
  67 #define GST_IS_BPWSINC(obj) \
  68       (G_TYPE_CHECK_INSTANCE_TYPE((obj),GST_TYPE_BPWSINC))
  69 #define GST_IS_BPWSINC_CLASS(obj) \
  70       (G_TYPE_CHECK_CLASS_TYPE((klass),GST_TYPE_BPWSINC))
  71
  72 typedef struct _GstBPWSinc GstBPWSinc;
  73 typedef struct _GstBPWSincClass GstBPWSincClass;
  74
  75 struct _GstBPWSinc
  76 {
  77   GstElement element;
  78
  79   GstPad *sinkpad, *srcpad;
  80
  81   double frequency;
  82   double lower_frequency, upper_frequency;
  83   int wing_size;                /* length of a "wing" of the filter;
  84                                    actual length is 2 * wing_size + 1 */
  85
  86   gfloat *residue;              /* buffer for left-over samples from previous buffer */
  87   double *kernel;
  88 };
  89
  90 struct _GstBPWSincClass
  91 {
  92   GstElementClass parent_class;
  93 };
  94
  95 static void gst_bpwsinc_base_init (gpointer g_class);
  96 static void gst_bpwsinc_class_init (GstBPWSincClass * klass);
  97 static void gst_bpwsinc_init (GstBPWSinc * filter);
  98
  99 static void gst_bpwsinc_set_property (GObject * object, guint prop_id,
 100     const GValue * value, GParamSpec * pspec);
 101 static void gst_bpwsinc_get_property (GObject * object, guint prop_id,
 102     GValue * value, GParamSpec * pspec);
 103
 104 static void gst_bpwsinc_chain (GstPad * pad, GstData * _data);
 105 static GstPadLinkReturn
 106 gst_bpwsinc_sink_connect (GstPad * pad, const GstCaps * caps);
 107
 108 static GstElementClass *parent_class = NULL;
 109
 110 /*static guint gst_bpwsinc_signals[LAST_SIGNAL] = { 0 }; */
 111
 112 GType
 113 gst_bpwsinc_get_type (void)
 114 {
 115   static GType bpwsinc_type = 0;
 116
 117   if (!bpwsinc_type) {
 118     static const GTypeInfo bpwsinc_info = {
 119       sizeof (GstBPWSincClass),
 120       gst_bpwsinc_base_init,
 121       NULL,
 122       (GClassInitFunc) gst_bpwsinc_class_init, NULL, NULL,
 123       sizeof (GstBPWSinc), 0,
 124       (GInstanceInitFunc) gst_bpwsinc_init,
 125     };
 126
 127     bpwsinc_type = g_type_register_static (GST_TYPE_ELEMENT, "GstBPWSinc",
 128         &bpwsinc_info, 0);
 129   }
 130   return bpwsinc_type;
 131 }
 132
 133 static void
 134 gst_bpwsinc_base_init (gpointer g_class)
 135 {
 136   GstElementClass *element_class = GST_ELEMENT_CLASS (g_class);
 137
 138   /* register src pads */
 139   gst_element_class_add_pad_template (element_class,
 140       gst_static_pad_template_get (&gst_filter_src_template));
 141   gst_element_class_add_pad_template (element_class,
 142       gst_static_pad_template_get (&gst_filter_sink_template));
 143
 144   gst_element_class_set_details (element_class, &gst_bpwsinc_details);
 145 }
 146
 147 static void
 148 gst_bpwsinc_class_init (GstBPWSincClass * klass)
 149 {
 150   GObjectClass *gobject_class;
 151   GstElementClass *gstelement_class;
 152
 153   gobject_class = (GObjectClass *) klass;
 154   gstelement_class = (GstElementClass *) klass;
 155
 156   parent_class = g_type_class_ref (GST_TYPE_ELEMENT);
 157
 158   g_object_class_install_property (G_OBJECT_CLASS (klass), ARG_LOWER_FREQUENCY,
 159       g_param_spec_double ("lower-frequency", "Lower Frequency",
 160           "Cut-off lower frequency (relative to sample rate)",
 161           0.0, 0.5, 0, G_PARAM_READWRITE));
 162   g_object_class_install_property (G_OBJECT_CLASS (klass), ARG_UPPER_FREQUENCY,
 163       g_param_spec_double ("upper-frequency", "Upper Frequency",
 164           "Cut-off upper frequency (relative to sample rate)",
 165           0.0, 0.5, 0, G_PARAM_READWRITE));
 166   g_object_class_install_property (G_OBJECT_CLASS (klass), ARG_LENGTH,
 167       g_param_spec_int ("length", "Length",
 168           "N such that the filter length = 2N + 1",
 169           1, G_MAXINT, 1, G_PARAM_READWRITE));
 170
 171   gobject_class->set_property = gst_bpwsinc_set_property;
 172   gobject_class->get_property = gst_bpwsinc_get_property;
 173 }
 174
 175 static void
 176 gst_bpwsinc_init (GstBPWSinc * filter)
 177 {
 178   filter->sinkpad =
 179       gst_pad_new_from_template (gst_static_pad_template_get
 180       (&gst_filter_sink_template), "sink");
 181   gst_pad_set_chain_function (filter->sinkpad, gst_bpwsinc_chain);
 182   gst_pad_set_link_function (filter->sinkpad, gst_bpwsinc_sink_connect);
 183   gst_element_add_pad (GST_ELEMENT (filter), filter->sinkpad);
 184
 185   filter->srcpad =
 186       gst_pad_new_from_template (gst_static_pad_template_get
 187       (&gst_filter_src_template), "src");
 188   gst_element_add_pad (GST_ELEMENT (filter), filter->srcpad);
 189
 190   filter->wing_size = 50;
 191   filter->lower_frequency = 0.25;
 192   filter->upper_frequency = 0.3;
 193   filter->kernel = NULL;
 194 }
 195
 196 static GstPadLinkReturn
 197 gst_bpwsinc_sink_connect (GstPad * pad, const GstCaps * caps)
 198 {
 199   int i = 0;
 200   double sum = 0.0;
 201   int len = 0;
 202   double *kernel_lp, *kernel_hp;
 203   GstPadLinkReturn set_retval;
 204
 205   GstBPWSinc *filter = GST_BPWSINC (gst_pad_get_parent (pad));
 206
 207   g_assert (GST_IS_PAD (pad));
 208   g_assert (caps != NULL);
 209
 210   set_retval = gst_pad_try_set_caps (filter->srcpad, caps);
 211
 212   if (set_retval > 0) {
 213     len = filter->wing_size;
 214     /* fill the lp kernel */
 215     GST_DEBUG ("bpwsinc: initializing LP kernel of length %d with cut-off %f",
 216         len * 2 + 1, filter->lower_frequency);
 217     kernel_lp = (double *) g_malloc (sizeof (double) * (2 * len + 1));
 218     for (i = 0; i <= len * 2; ++i) {
 219       if (i == len)
 220         kernel_lp[i] = 2 * M_PI * filter->lower_frequency;
 221       else
 222         kernel_lp[i] = sin (2 * M_PI * filter->lower_frequency * (i - len))
 223             / (i - len);
 224       /* Blackman windowing */
 225       kernel_lp[i] *= (0.42 - 0.5 * cos (M_PI * i / len)
 226           + 0.08 * cos (2 * M_PI * i / len));
 227     }
 228
 229     /* normalize for unity gain at DC
 230      * FIXME: sure this is not supposed to be quadratic ? */
 231     sum = 0.0;
 232     for (i = 0; i <= len * 2; ++i)
 233       sum += kernel_lp[i];
 234     for (i = 0; i <= len * 2; ++i)
 235       kernel_lp[i] /= sum;
 236
 237     /* fill the hp kernel */
 238     GST_DEBUG ("bpwsinc: initializing HP kernel of length %d with cut-off %f",
 239         len * 2 + 1, filter->upper_frequency);
 240     kernel_hp = (double *) g_malloc (sizeof (double) * (2 * len + 1));
 241     for (i = 0; i <= len * 2; ++i) {
 242       if (i == len)
 243         kernel_hp[i] = 2 * M_PI * filter->upper_frequency;
 244       else
 245         kernel_hp[i] = sin (2 * M_PI * filter->upper_frequency * (i - len))
 246             / (i - len);
 247       /* Blackman windowing */
 248       kernel_hp[i] *= (0.42 - 0.5 * cos (M_PI * i / len)
 249           + 0.08 * cos (2 * M_PI * i / len));
 250     }
 251
 252     /* normalize for unity gain at DC
 253      * FIXME: sure this is not supposed to be quadratic ? */
 254     sum = 0.0;
 255     for (i = 0; i <= len * 2; ++i)
 256       sum += kernel_hp[i];
 257     for (i = 0; i <= len * 2; ++i)
 258       kernel_hp[i] /= sum;
 259
 260     /* do spectral inversion to get a HP filter */
 261     for (i = 0; i <= len * 2; ++i)
 262       kernel_hp[i] = -kernel_hp[i];
 263     kernel_hp[len] += 1;
 264
 265     /* combine the two filters */
 266
 267     filter->kernel = (double *) g_malloc (sizeof (double) * (2 * len + 1));
 268
 269     for (i = 0; i <= len * 2; ++i)
 270       filter->kernel[i] = kernel_lp[i] + kernel_hp[i];
 271
 272     /* do spectral inversion to go from band reject to bandpass */
 273     for (i = 0; i <= len * 2; ++i)
 274       filter->kernel[i] = -filter->kernel[i];
 275     filter->kernel[len] += 1;
 276
 277     /* free the helper kernels */
 278     g_free (kernel_lp);
 279     g_free (kernel_hp);
 280
 281     /* set up the residue memory space */
 282     filter->residue = (gfloat *) g_malloc (sizeof (gfloat) * (len * 2 + 1));
 283     for (i = 0; i <= len * 2; ++i)
 284       filter->residue[i] = 0.0;
 285   }
 286
 287   return set_retval;
 288 }
 289
 290 static void
 291 gst_bpwsinc_chain (GstPad * pad, GstData * _data)
 292 {
 293   GstBuffer *buf = GST_BUFFER (_data);
 294   GstBPWSinc *filter;
 295   gfloat *src;
 296   gfloat *input;
 297   gint residue_samples;
 298   gint input_samples;
 299   gint total_samples;
 300   int i, j;
 301
 302   filter = GST_BPWSINC (gst_pad_get_parent (pad));
 303
 304   /* FIXME: out of laziness, we copy the left-over bit from last buffer
 305    * together with the incoming buffer to a new buffer to make the loop
 306    * easy; this could be a lot more optimized though
 307    * to make amends we keep the incoming buffer around and write our
 308    * output samples there */
 309
 310   /* get a writable buffer */
 311   buf = gst_buffer_copy_on_write (buf);
 312
 313   src = (gfloat *) GST_BUFFER_DATA (buf);
 314   residue_samples = filter->wing_size * 2 + 1;
 315   input_samples = GST_BUFFER_SIZE (buf) / sizeof (gfloat);
 316   total_samples = residue_samples + input_samples;
 317
 318   input = (gfloat *) g_malloc (sizeof (gfloat) * total_samples);
 319
 320   /* copy the left-over bit */
 321   memcpy (input, filter->residue, sizeof (gfloat) * residue_samples);
 322
 323   /* copy the new buffer */
 324   memcpy (&input[residue_samples], src, sizeof (gfloat) * input_samples);
 325   /* copy the tail of the current input buffer to the residue */
 326   memcpy (filter->residue, &src[input_samples - residue_samples],
 327       sizeof (gfloat) * residue_samples);
 328
 329   /* convolution */
 330   /* since we copied the previous set of samples we needed before the actual
 331    * input data, we need to add the filter length to our indices for input */
 332   for (i = 0; i < input_samples; ++i) {
 333     src[i] = 0.0;
 334     for (j = 0; j < residue_samples; ++j)
 335       src[i] += input[i - j + residue_samples] * filter->kernel[j];
 336   }
 337
 338   g_free (input);
 339   gst_pad_push (filter->srcpad, GST_DATA (buf));
 340 }
 341
 342 static void
 343 gst_bpwsinc_set_property (GObject * object, guint prop_id, const GValue * value,
 344     GParamSpec * pspec)
 345 {
 346   GstBPWSinc *filter;
 347
 348   /* it's not null if we got it, but it might not be ours */
 349   g_return_if_fail (GST_IS_BPWSINC (object));
 350
 351   filter = GST_BPWSINC (object);
 352
 353   switch (prop_id) {
 354     case ARG_LENGTH:
 355       filter->wing_size = g_value_get_int (value);
 356       break;
 357     case ARG_LOWER_FREQUENCY:
 358       filter->lower_frequency = g_value_get_double (value);
 359       break;
 360     case ARG_UPPER_FREQUENCY:
 361       filter->upper_frequency = g_value_get_double (value);
 362       break;
 363     default:
 364       break;
 365   }
 366 }
 367
 368 static void
 369 gst_bpwsinc_get_property (GObject * object, guint prop_id, GValue * value,
 370     GParamSpec * pspec)
 371 {
 372   GstBPWSinc *filter;
 373
 374   /* it's not null if we got it, but it might not be ours */
 375   g_return_if_fail (GST_IS_BPWSINC (object));
 376
 377   filter = GST_BPWSINC (object);
 378
 379   switch (prop_id) {
 380     case ARG_LENGTH:
 381       g_value_set_int (value, filter->wing_size);
 382       break;
 383     case ARG_LOWER_FREQUENCY:
 384       g_value_set_double (value, filter->lower_frequency);
 385       break;
 386     case ARG_UPPER_FREQUENCY:
 387       g_value_set_double (value, filter->upper_frequency);
 388       break;
 389     default:
 390       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
 391       break;
 392   }
 393 }