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
gst_audio_fx_base_iir_filter_finalize(GObject * object)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
gst_audio_fx_base_iir_filter_class_init(GstAudioFXBaseIIRFilterClass * klass)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
123 static void
gst_audio_fx_base_iir_filter_init(GstAudioFXBaseIIRFilter * filter)124 gst_audio_fx_base_iir_filter_init (GstAudioFXBaseIIRFilter * filter)
125 {
126 gst_base_transform_set_in_place (GST_BASE_TRANSFORM (filter), TRUE);
127
128 filter->a = NULL;
129 filter->na = 0;
130 filter->b = NULL;
131 filter->nb = 0;
132 filter->channels = NULL;
133 filter->nchannels = 0;
134
135 g_mutex_init (&filter->lock);
136 }
137
138 /* Evaluate the transfer function that corresponds to the IIR
139 * coefficients at (zr + zi*I)^-1 and return the magnitude */
140 gdouble
gst_audio_fx_base_iir_filter_calculate_gain(gdouble * a,guint na,gdouble * b,guint nb,gdouble zr,gdouble zi)141 gst_audio_fx_base_iir_filter_calculate_gain (gdouble * a, guint na, gdouble * b,
142 guint nb, gdouble zr, gdouble zi)
143 {
144 gdouble sum_ar, sum_ai;
145 gdouble sum_br, sum_bi;
146 gdouble gain_r, gain_i;
147
148 gdouble sum_r_old;
149 gdouble sum_i_old;
150
151 gint i;
152
153 sum_ar = a[na - 1];
154 sum_ai = 0.0;
155 for (i = na - 2; i >= 0; i--) {
156 sum_r_old = sum_ar;
157 sum_i_old = sum_ai;
158
159 sum_ar = (sum_r_old * zr - sum_i_old * zi) + a[i];
160 sum_ai = (sum_r_old * zi + sum_i_old * zr) + 0.0;
161 }
162
163 sum_br = b[nb - 1];
164 sum_bi = 0.0;
165 for (i = nb - 2; i >= 0; i--) {
166 sum_r_old = sum_br;
167 sum_i_old = sum_bi;
168
169 sum_br = (sum_r_old * zr - sum_i_old * zi) + b[i];
170 sum_bi = (sum_r_old * zi + sum_i_old * zr) + 0.0;
171 }
172
173 gain_r =
174 (sum_br * sum_ar + sum_bi * sum_ai) / (sum_ar * sum_ar + sum_ai * sum_ai);
175 gain_i =
176 (sum_bi * sum_ar - sum_br * sum_ai) / (sum_ar * sum_ar + sum_ai * sum_ai);
177
178 return (sqrt (gain_r * gain_r + gain_i * gain_i));
179 }
180
181 void
gst_audio_fx_base_iir_filter_set_coefficients(GstAudioFXBaseIIRFilter * filter,gdouble * a,guint na,gdouble * b,guint nb)182 gst_audio_fx_base_iir_filter_set_coefficients (GstAudioFXBaseIIRFilter * filter,
183 gdouble * a, guint na, gdouble * b, guint nb)
184 {
185 guint i;
186
187 g_return_if_fail (GST_IS_AUDIO_FX_BASE_IIR_FILTER (filter));
188
189 g_mutex_lock (&filter->lock);
190
191 g_free (filter->a);
192 g_free (filter->b);
193
194 filter->a = filter->b = NULL;
195
196 if (filter->channels) {
197 GstAudioFXBaseIIRFilterChannelCtx *ctx;
198 gboolean free = (na != filter->na || nb != filter->nb);
199
200 for (i = 0; i < filter->nchannels; i++) {
201 ctx = &filter->channels[i];
202
203 if (free)
204 g_free (ctx->x);
205 else
206 memset (ctx->x, 0, filter->nb * sizeof (gdouble));
207
208 if (free)
209 g_free (ctx->y);
210 else
211 memset (ctx->y, 0, filter->na * sizeof (gdouble));
212 }
213
214 g_free (filter->channels);
215 filter->channels = NULL;
216 }
217
218 filter->na = na;
219 filter->nb = nb;
220
221 filter->a = a;
222 filter->b = b;
223
224 if (filter->nchannels && !filter->channels) {
225 GstAudioFXBaseIIRFilterChannelCtx *ctx;
226
227 filter->channels =
228 g_new0 (GstAudioFXBaseIIRFilterChannelCtx, filter->nchannels);
229 for (i = 0; i < filter->nchannels; i++) {
230 ctx = &filter->channels[i];
231
232 ctx->x = g_new0 (gdouble, filter->nb);
233 ctx->y = g_new0 (gdouble, filter->na);
234 }
235 }
236
237 g_mutex_unlock (&filter->lock);
238 }
239
240 /* GstAudioFilter vmethod implementations */
241
242 static gboolean
gst_audio_fx_base_iir_filter_setup(GstAudioFilter * base,const GstAudioInfo * info)243 gst_audio_fx_base_iir_filter_setup (GstAudioFilter * base,
244 const GstAudioInfo * info)
245 {
246 GstAudioFXBaseIIRFilter *filter = GST_AUDIO_FX_BASE_IIR_FILTER (base);
247 gboolean ret = TRUE;
248 gint channels;
249
250 g_mutex_lock (&filter->lock);
251 switch (GST_AUDIO_INFO_FORMAT (info)) {
252 case GST_AUDIO_FORMAT_F32:
253 filter->process = (GstAudioFXBaseIIRFilterProcessFunc)
254 process_32;
255 break;
256 case GST_AUDIO_FORMAT_F64:
257 filter->process = (GstAudioFXBaseIIRFilterProcessFunc)
258 process_64;
259 break;
260 default:
261 ret = FALSE;
262 break;
263 }
264
265 channels = GST_AUDIO_INFO_CHANNELS (info);
266
267 if (channels != filter->nchannels) {
268 guint i;
269 GstAudioFXBaseIIRFilterChannelCtx *ctx;
270
271 if (filter->channels) {
272 for (i = 0; i < filter->nchannels; i++) {
273 ctx = &filter->channels[i];
274
275 g_free (ctx->x);
276 g_free (ctx->y);
277 }
278 g_free (filter->channels);
279 }
280
281 filter->channels = g_new0 (GstAudioFXBaseIIRFilterChannelCtx, channels);
282 for (i = 0; i < channels; i++) {
283 ctx = &filter->channels[i];
284
285 ctx->x = g_new0 (gdouble, filter->nb);
286 ctx->y = g_new0 (gdouble, filter->na);
287 }
288 filter->nchannels = channels;
289 }
290 g_mutex_unlock (&filter->lock);
291
292 return ret;
293 }
294
295 static inline gdouble
process(GstAudioFXBaseIIRFilter * filter,GstAudioFXBaseIIRFilterChannelCtx * ctx,gdouble x0)296 process (GstAudioFXBaseIIRFilter * filter,
297 GstAudioFXBaseIIRFilterChannelCtx * ctx, gdouble x0)
298 {
299 gdouble val = filter->b[0] * x0;
300 gint i, j;
301
302 for (i = 1, j = ctx->x_pos; i < filter->nb; i++) {
303 val += filter->b[i] * ctx->x[j];
304 j--;
305 if (j < 0)
306 j = filter->nb - 1;
307 }
308
309 for (i = 1, j = ctx->y_pos; i < filter->na; i++) {
310 val -= filter->a[i] * ctx->y[j];
311 j--;
312 if (j < 0)
313 j = filter->na - 1;
314 }
315 val /= filter->a[0];
316
317 if (ctx->x) {
318 ctx->x_pos++;
319 if (ctx->x_pos >= filter->nb)
320 ctx->x_pos = 0;
321 ctx->x[ctx->x_pos] = x0;
322 }
323 if (ctx->y) {
324 ctx->y_pos++;
325 if (ctx->y_pos >= filter->na)
326 ctx->y_pos = 0;
327
328 ctx->y[ctx->y_pos] = val;
329 }
330
331 return val;
332 }
333
334 #define DEFINE_PROCESS_FUNC(width,ctype) \
335 static void \
336 process_##width (GstAudioFXBaseIIRFilter * filter, \
337 g##ctype * data, guint num_samples) \
338 { \
339 gint i, j, channels = filter->nchannels; \
340 gdouble val; \
341 \
342 for (i = 0; i < num_samples / channels; i++) { \
343 for (j = 0; j < channels; j++) { \
344 val = process (filter, &filter->channels[j], *data); \
345 *data++ = val; \
346 } \
347 } \
348 }
349
350 DEFINE_PROCESS_FUNC (32, float);
351 DEFINE_PROCESS_FUNC (64, double);
352
353 #undef DEFINE_PROCESS_FUNC
354
355 /* GstBaseTransform vmethod implementations */
356 static GstFlowReturn
gst_audio_fx_base_iir_filter_transform_ip(GstBaseTransform * base,GstBuffer * buf)357 gst_audio_fx_base_iir_filter_transform_ip (GstBaseTransform * base,
358 GstBuffer * buf)
359 {
360 GstAudioFXBaseIIRFilter *filter = GST_AUDIO_FX_BASE_IIR_FILTER (base);
361 guint num_samples;
362 GstClockTime timestamp, stream_time;
363 GstMapInfo map;
364
365 timestamp = GST_BUFFER_TIMESTAMP (buf);
366 stream_time =
367 gst_segment_to_stream_time (&base->segment, GST_FORMAT_TIME, timestamp);
368
369 GST_DEBUG_OBJECT (filter, "sync to %" GST_TIME_FORMAT,
370 GST_TIME_ARGS (timestamp));
371
372 if (GST_CLOCK_TIME_IS_VALID (stream_time))
373 gst_object_sync_values (GST_OBJECT (filter), stream_time);
374
375 gst_buffer_map (buf, &map, GST_MAP_READWRITE);
376 num_samples = map.size / GST_AUDIO_FILTER_BPS (filter);
377
378 g_mutex_lock (&filter->lock);
379 if (filter->a == NULL || filter->b == NULL) {
380 g_warn_if_fail (filter->a != NULL && filter->b != NULL);
381 gst_buffer_unmap (buf, &map);
382 g_mutex_unlock (&filter->lock);
383 return GST_FLOW_ERROR;
384 }
385 filter->process (filter, map.data, num_samples);
386 g_mutex_unlock (&filter->lock);
387
388 gst_buffer_unmap (buf, &map);
389
390 return GST_FLOW_OK;
391 }
392
393
394 static gboolean
gst_audio_fx_base_iir_filter_stop(GstBaseTransform * base)395 gst_audio_fx_base_iir_filter_stop (GstBaseTransform * base)
396 {
397 GstAudioFXBaseIIRFilter *filter = GST_AUDIO_FX_BASE_IIR_FILTER (base);
398 guint channels = filter->nchannels;
399 GstAudioFXBaseIIRFilterChannelCtx *ctx;
400 guint i;
401
402 /* Reset the history of input and output values if
403 * already existing */
404 if (channels && filter->channels) {
405 for (i = 0; i < channels; i++) {
406 ctx = &filter->channels[i];
407 g_free (ctx->x);
408 g_free (ctx->y);
409 }
410 g_free (filter->channels);
411 }
412 filter->channels = NULL;
413 filter->nchannels = 0;
414
415 return TRUE;
416 }
417