audioconvert: add LFE filter

Use a lowpass filter to generate LFE from the stereo channels.

spa/plugins/audioconvert/biquad.c

@@ -0,0 +1,115 @@
+/* Copyright (c) 2013 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/* Copyright (C) 2010 Google Inc. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE.WEBKIT file.
+ */
+
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <spa/utils/defs.h>
+
+#include <math.h>
+#include "biquad.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+#ifndef M_SQRT2
+#define M_SQRT2 1.41421356237309504880
+#endif
+
+static void set_coefficient(struct biquad *bq, double b0, double b1, double b2,
+			    double a0, double a1, double a2)
+{
+	double a0_inv = 1 / a0;
+	bq->b0 = b0 * a0_inv;
+	bq->b1 = b1 * a0_inv;
+	bq->b2 = b2 * a0_inv;
+	bq->a1 = a1 * a0_inv;
+	bq->a2 = a2 * a0_inv;
+}
+
+static void biquad_lowpass(struct biquad *bq, double cutoff)
+{
+	/* Limit cutoff to 0 to 1. */
+	cutoff = SPA_CLAMP(cutoff, 0.0, 1.0);
+
+	if (cutoff >= 1.0) {
+		/* When cutoff is 1, the z-transform is 1. */
+		set_coefficient(bq, 1, 0, 0, 1, 0, 0);
+	} else if (cutoff > 0) {
+		/* Compute biquad coefficients for lowpass filter */
+		double theta = M_PI * cutoff;
+		double sn = 0.5 * M_SQRT2 * sin(theta);
+		double beta = 0.5 * (1 - sn) / (1 + sn);
+		double gamma_coeff = (0.5 + beta) * cos(theta);
+		double alpha = 0.25 * (0.5 + beta - gamma_coeff);
+
+		double b0 = 2 * alpha;
+		double b1 = 2 * 2 * alpha;
+		double b2 = 2 * alpha;
+		double a1 = 2 * -gamma_coeff;
+		double a2 = 2 * beta;
+
+		set_coefficient(bq, b0, b1, b2, 1, a1, a2);
+	} else {
+		/* When cutoff is zero, nothing gets through the filter, so set
+		 * coefficients up correctly.
+		 */
+		set_coefficient(bq, 0, 0, 0, 1, 0, 0);
+	}
+}
+
+static void biquad_highpass(struct biquad *bq, double cutoff)
+{
+	/* Limit cutoff to 0 to 1. */
+	cutoff = SPA_CLAMP(cutoff, 0.0, 1.0);
+
+	if (cutoff >= 1.0) {
+		/* The z-transform is 0. */
+		set_coefficient(bq, 0, 0, 0, 1, 0, 0);
+	} else if (cutoff > 0) {
+		/* Compute biquad coefficients for highpass filter */
+		double theta = M_PI * cutoff;
+		double sn = 0.5 * M_SQRT2 * sin(theta);
+		double beta = 0.5 * (1 - sn) / (1 + sn);
+		double gamma_coeff = (0.5 + beta) * cos(theta);
+		double alpha = 0.25 * (0.5 + beta + gamma_coeff);
+
+		double b0 = 2 * alpha;
+		double b1 = 2 * -2 * alpha;
+		double b2 = 2 * alpha;
+		double a1 = 2 * -gamma_coeff;
+		double a2 = 2 * beta;
+
+		set_coefficient(bq, b0, b1, b2, 1, a1, a2);
+	} else {
+		/* When cutoff is zero, we need to be careful because the above
+		 * gives a quadratic divided by the same quadratic, with poles
+		 * and zeros on the unit circle in the same place. When cutoff
+		 * is zero, the z-transform is 1.
+		 */
+		set_coefficient(bq, 1, 0, 0, 1, 0, 0);
+	}
+}
+
+void biquad_set(struct biquad *bq, enum biquad_type type, double freq)
+{
+
+	switch (type) {
+	case BQ_LOWPASS:
+		biquad_lowpass(bq, freq);
+		break;
+	case BQ_HIGHPASS:
+		biquad_highpass(bq, freq);
+		break;
+	}
+}

spa/plugins/audioconvert/biquad.h

@@ -0,0 +1,45 @@
+/* Copyright (c) 2013 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#ifndef BIQUAD_H_
+#define BIQUAD_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* The biquad filter parameters. The transfer function H(z) is (b0 + b1 * z^(-1)
+ * + b2 * z^(-2)) / (1 + a1 * z^(-1) + a2 * z^(-2)).  The previous two inputs
+ * are stored in x1 and x2, and the previous two outputs are stored in y1 and
+ * y2.
+ *
+ * We use double during the coefficients calculation for better accurary, but
+ * float is used during the actual filtering for faster computation.
+ */
+struct biquad {
+	float b0, b1, b2;
+	float a1, a2;
+};
+
+/* The type of the biquad filters */
+enum biquad_type {
+	BQ_LOWPASS,
+	BQ_HIGHPASS,
+};
+
+/* Initialize a biquad filter parameters from its type and parameters.
+ * Args:
+ *    bq - The biquad filter we want to set.
+ *    type - The type of the biquad filter.
+ *    frequency - The value should be in the range [0, 1]. It is relative to
+ *        half of the sampling rate.
+ */
+void biquad_set(struct biquad *bq, enum biquad_type type, double freq);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif /* BIQUAD_H_ */

spa/plugins/audioconvert/channelmix-ops-c.c

@@ -252,6 +252,8 @@ channelmix_f32_2_3p1_c(struct channelmix *mix, uint32_t n_dst, void * SPA_RESTRI
 			d[2][n] = c * v2;
 			d[3][n] = c * v3;
 		}
+		if (v3 > 0.0f)
+			lr4_process(&mix->lr4[3], d[3], n_samples);
 	}
 	else {
 		for (n = 0; n < n_samples; n++) {
@@ -261,6 +263,8 @@ channelmix_f32_2_3p1_c(struct channelmix *mix, uint32_t n_dst, void * SPA_RESTRI
 			d[2][n] = c * v2;
 			d[3][n] = c * v3;
 		}
+		if (v3 > 0.0f)
+			lr4_process(&mix->lr4[3], d[3], n_samples);
 	}
 }
 
@@ -291,6 +295,8 @@ channelmix_f32_2_5p1_c(struct channelmix *mix, uint32_t n_dst, void * SPA_RESTRI
 			d[2][n] = c * v2;
 			d[3][n] = c * v3;
 		}
+		if (v3 > 0.0f)
+			lr4_process(&mix->lr4[3], d[3], n_samples);
 	}
 	else {
 		for (n = 0; n < n_samples; n++) {
@@ -302,6 +308,8 @@ channelmix_f32_2_5p1_c(struct channelmix *mix, uint32_t n_dst, void * SPA_RESTRI
 			d[4][n] = s[0][n] * v4;
 			d[5][n] = s[1][n] * v5;
 		}
+		if (v3 > 0.0f)
+			lr4_process(&mix->lr4[3], d[3], n_samples);
 	}
 }
 

spa/plugins/audioconvert/channelmix-ops.c

@@ -429,6 +429,8 @@ done:
 				continue;
 			mix->matrix_orig[ic][jc++] = matrix[i][j];
 			sum += fabs(matrix[i][j]);
+			if (i == LFE)
+				lr4_set(&mix->lr4[ic], BQ_LOWPASS, mix->lfe_cutoff / mix->freq);
 		}
 		maxsum = SPA_MAX(maxsum, sum);
 		ic++;

spa/plugins/audioconvert/channelmix-ops.h

@@ -28,6 +28,8 @@
 #include <spa/utils/defs.h>
 #include <spa/param/audio/raw.h>
 
+#include "crossover.h"
+
 #define VOLUME_MIN 0.0f
 #define VOLUME_NORM 1.0f
 
@@ -63,6 +65,7 @@ struct channelmix {
 
 	float freq;					/* sample frequency */
 	float lfe_cutoff;				/* in Hz, 0 is disabled */
+	struct lr4 lr4[SPA_AUDIO_MAX_CHANNELS];
 
 	void (*process) (struct channelmix *mix, uint32_t n_dst, void * SPA_RESTRICT dst[n_dst],
 			uint32_t n_src, const void * SPA_RESTRICT src[n_src], uint32_t n_samples);

spa/plugins/audioconvert/crossover.c

@@ -0,0 +1,58 @@
+/* Copyright (c) 2013 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include "crossover.h"
+
+void lr4_set(struct lr4 *lr4, enum biquad_type type, float freq)
+{
+	biquad_set(&lr4->bq, type, freq);
+	lr4->x1 = 0;
+	lr4->x2 = 0;
+	lr4->y1 = 0;
+	lr4->y2 = 0;
+	lr4->z1 = 0;
+	lr4->z2 = 0;
+}
+
+void lr4_process(struct lr4 *lr4, float *data, int samples)
+{
+	float lx1 = lr4->x1;
+	float lx2 = lr4->x2;
+	float ly1 = lr4->y1;
+	float ly2 = lr4->y2;
+	float lz1 = lr4->z1;
+	float lz2 = lr4->z2;
+	float lb0 = lr4->bq.b0;
+	float lb1 = lr4->bq.b1;
+	float lb2 = lr4->bq.b2;
+	float la1 = lr4->bq.a1;
+	float la2 = lr4->bq.a2;
+
+	int i;
+	for (i = 0; i < samples; i++) {
+		float x, y, z;
+		x = data[i];
+		y = lb0*x + lb1*lx1 + lb2*lx2 - la1*ly1 - la2*ly2;
+		z = lb0*y + lb1*ly1 + lb2*ly2 - la1*lz1 - la2*lz2;
+		lx2 = lx1;
+		lx1 = x;
+		ly2 = ly1;
+		ly1 = y;
+		lz2 = lz1;
+		lz1 = z;
+		data[i] = z;
+	}
+
+	lr4->x1 = lx1;
+	lr4->x2 = lx2;
+	lr4->y1 = ly1;
+	lr4->y2 = ly2;
+	lr4->z1 = lz1;
+	lr4->z2 = lz2;
+}

spa/plugins/audioconvert/crossover.h

@@ -0,0 +1,28 @@
+/* Copyright (c) 2013 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#ifndef CROSSOVER_H_
+#define CROSSOVER_H_
+
+#include "biquad.h"
+/* An LR4 filter is two biquads with the same parameters connected in series:
+ *
+ * x -- [BIQUAD] -- y -- [BIQUAD] -- z
+ *
+ * Both biquad filter has the same parameter b[012] and a[12],
+ * The variable [xyz][12] keep the history values.
+ */
+struct lr4 {
+	struct biquad bq;
+	float x1, x2;
+	float y1, y2;
+	float z1, z2;
+};
+
+void lr4_set(struct lr4 *lr4, enum biquad_type type, float freq);
+
+void lr4_process(struct lr4 *lr4, float *data, int samples);
+
+#endif /* CROSSOVER_H_ */

spa/plugins/audioconvert/meson.build

@@ -89,6 +89,8 @@ endif
 
 audioconvert = static_library('audioconvert',
 	['fmt-ops.c',
+	 'biquad.c',
+         'crossover.c',
 	 'channelmix-ops.c',
 	 'channelmix-ops-c.c',
 	 'resample-native.c',