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