lfe-filter: Enable LFE filter in the resampler

When enable-lfe-remixing is set, an LFE channel is present in the
resampler's destination channel map but not in the source channel map,
we insert a low-pass filter instead of just averaging the channels.
Other channels will get a high-pass filter.

In this patch, the crossover frequency is hardcoded to 120Hz (to be fixed
in later patches).

Note that in current state the LFE filter is
 - not very optimised
 - not rewind friendly (rewinding can cause audible artifacts)

Signed-off-by: David Henningsson <david.henningsson@canonical.com>

7 files changed, 265 insertions, 5 deletions

diff --git a/src/Makefile.am b/src/Makefile.am
index 67f862770..302c532d2 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -911,6 +911,9 @@ lib_LTLIBRARIES += libpulsecore-@PA_MAJORMINOR@.la
 
 # Pure core stuff
 libpulsecore_@PA_MAJORMINOR@_la_SOURCES = \
+		pulsecore/filter/lfe-filter.c pulsecore/filter/lfe-filter.h \
+		pulsecore/filter/biquad.c pulsecore/filter/biquad.h \
+		pulsecore/filter/crossover.c pulsecore/filter/crossover.h \
 		pulsecore/asyncmsgq.c pulsecore/asyncmsgq.h \
 		pulsecore/asyncq.c pulsecore/asyncq.h \
 		pulsecore/auth-cookie.c pulsecore/auth-cookie.h \
diff --git a/src/pulsecore/filter/crossover.c b/src/pulsecore/filter/crossover.c
index 11a8c6e01..0a571c3e8 100644
--- a/src/pulsecore/filter/crossover.c
+++ b/src/pulsecore/filter/crossover.c
@@ -3,10 +3,16 @@
  * found in the LICENSE file.
  */
 
-#include "crossover.h"
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <pulsecore/macro.h>
+
 #include "biquad.h"
+#include "crossover.h"
 
-static void lr4_set(struct lr4 *lr4, enum biquad_type type, float freq)
+void lr4_set(struct lr4 *lr4, enum biquad_type type, float freq)
 {
 	struct biquad q;
 	biquad_set(&q, type, freq, 0, 0);
@@ -23,6 +29,81 @@ static void lr4_set(struct lr4 *lr4, enum biquad_type type, float freq)
 	lr4->z2 = 0;
 }
 
+void lr4_process_float32(struct lr4 *lr4, int samples, int channels, float *src, float *dest)
+{
+	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->b0;
+	float lb1 = lr4->b1;
+	float lb2 = lr4->b2;
+	float la1 = lr4->a1;
+	float la2 = lr4->a2;
+
+	int i;
+	for (i = 0; i < samples * channels; i += channels) {
+		float x, y, z;
+		x = src[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;
+		dest[i] = z;
+	}
+
+	lr4->x1 = lx1;
+	lr4->x2 = lx2;
+	lr4->y1 = ly1;
+	lr4->y2 = ly2;
+	lr4->z1 = lz1;
+	lr4->z2 = lz2;
+}
+
+void lr4_process_s16(struct lr4 *lr4, int samples, int channels, short *src, short *dest)
+{
+	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->b0;
+	float lb1 = lr4->b1;
+	float lb2 = lr4->b2;
+	float la1 = lr4->a1;
+	float la2 = lr4->a2;
+
+	int i;
+	for (i = 0; i < samples * channels; i += channels) {
+		float x, y, z;
+		x = src[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;
+		dest[i] = PA_CLAMP_UNLIKELY((int) z, -0x8000, 0x7fff);
+	}
+
+	lr4->x1 = lx1;
+	lr4->x2 = lx2;
+	lr4->y1 = ly1;
+	lr4->y2 = ly2;
+	lr4->z1 = lz1;
+	lr4->z2 = lz2;
+}
+
+
 /* Split input data using two LR4 filters, put the result into the input array
  * and another array.
  *
diff --git a/src/pulsecore/filter/crossover.h b/src/pulsecore/filter/crossover.h
index 99a601c13..a88f5b6cf 100644
--- a/src/pulsecore/filter/crossover.h
+++ b/src/pulsecore/filter/crossover.h
@@ -25,6 +25,12 @@ struct lr4 {
 	float z1, z2;
 };
 
+void lr4_set(struct lr4 *lr4, enum biquad_type type, float freq);
+
+void lr4_process_float32(struct lr4 *lr4, int samples, int channels, float *src, float *dest);
+void lr4_process_s16(struct lr4 *lr4, int samples, int channels, short *src, short *dest);
+
+
 /* Three bands crossover filter:
  *
  * INPUT --+-- lp0 --+-- lp1 --+---> LOW (0)
diff --git a/src/pulsecore/filter/lfe-filter.c b/src/pulsecore/filter/lfe-filter.c
new file mode 100644
index 000000000..e2bc3293d
--- /dev/null
+++ b/src/pulsecore/filter/lfe-filter.c
@@ -0,0 +1,101 @@
+/***
+  This file is part of PulseAudio.
+
+  Copyright 2014 David Henningsson, Canonical Ltd.
+
+  PulseAudio is free software; you can redistribute it and/or modify
+  it under the terms of the GNU Lesser General Public License as published
+  by the Free Software Foundation; either version 2.1 of the License,
+  or (at your option) any later version.
+
+  PulseAudio is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public License
+  along with PulseAudio; if not, see <http://www.gnu.org/licenses/>.
+***/
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include "lfe-filter.h"
+#include <pulse/xmalloc.h>
+#include <pulsecore/filter/biquad.h>
+#include <pulsecore/filter/crossover.h>
+
+/* An LR4 filter, implemented as a chain of two Butterworth filters.
+
+   Currently the channel map is fixed so that a highpass filter is applied to all
+   channels except for the LFE channel, where a lowpass filter is applied.
+   This works well for e g stereo to 2.1/5.1/7.1 scenarios, where the remap engine
+   has calculated the LFE channel to be the average of all source channels.
+*/
+
+struct pa_lfe_filter {
+    float crossover;
+    pa_channel_map cm;
+    pa_sample_spec ss;
+    bool active;
+    struct lr4 lr4[PA_CHANNELS_MAX];
+};
+
+pa_lfe_filter_t * pa_lfe_filter_new(const pa_sample_spec* ss, const pa_channel_map* cm, float crossover_freq) {
+
+    pa_lfe_filter_t *f = pa_xnew0(struct pa_lfe_filter, 1);
+    f->crossover = crossover_freq;
+    f->cm = *cm;
+    f->ss = *ss;
+    pa_lfe_filter_update_rate(f, ss->rate);
+    return f;
+}
+
+void pa_lfe_filter_free(pa_lfe_filter_t *f) {
+    pa_xfree(f);
+}
+
+void pa_lfe_filter_reset(pa_lfe_filter_t *f) {
+    pa_lfe_filter_update_rate(f, f->ss.rate);
+}
+
+pa_memchunk * pa_lfe_filter_process(pa_lfe_filter_t *f, pa_memchunk *buf) {
+    int samples = buf->length / pa_frame_size(&f->ss);
+
+    if (!f->active)
+        return buf;
+    if (f->ss.format == PA_SAMPLE_FLOAT32NE) {
+        int i;
+        float *data = pa_memblock_acquire_chunk(buf);
+        for (i = 0; i < f->cm.channels; i++)
+            lr4_process_float32(&f->lr4[i], samples, f->cm.channels, &data[i], &data[i]);
+        pa_memblock_release(buf->memblock);
+    }
+    else if (f->ss.format == PA_SAMPLE_S16NE) {
+        int i;
+        short *data = pa_memblock_acquire_chunk(buf);
+        for (i = 0; i < f->cm.channels; i++)
+            lr4_process_s16(&f->lr4[i], samples, f->cm.channels, &data[i], &data[i]);
+        pa_memblock_release(buf->memblock);
+    }
+    else pa_assert_not_reached();
+    return buf;
+}
+
+void pa_lfe_filter_update_rate(pa_lfe_filter_t *f, uint32_t new_rate) {
+    int i;
+    float biquad_freq = f->crossover / (new_rate / 2);
+
+    f->ss.rate = new_rate;
+    if (biquad_freq <= 0 || biquad_freq >= 1) {
+        pa_log_warn("Crossover frequency (%f) outside range for sample rate %d", f->crossover, new_rate);
+        f->active = false;
+        return;
+    }
+
+    for (i = 0; i < f->cm.channels; i++)
+        lr4_set(&f->lr4[i], f->cm.map[i] == PA_CHANNEL_POSITION_LFE ? BQ_LOWPASS : BQ_HIGHPASS, biquad_freq);
+
+    f->active = true;
+}
diff --git a/src/pulsecore/filter/lfe-filter.h b/src/pulsecore/filter/lfe-filter.h
new file mode 100644
index 000000000..25db8a0ec
--- /dev/null
+++ b/src/pulsecore/filter/lfe-filter.h
@@ -0,0 +1,38 @@
+#ifndef foolfefilterhfoo
+#define foolfefilterhfoo
+
+/***
+  This file is part of PulseAudio.
+
+  Copyright 2014 David Henningsson, Canonical Ltd.
+
+  PulseAudio is free software; you can redistribute it and/or modify
+  it under the terms of the GNU Lesser General Public License as published
+  by the Free Software Foundation; either version 2.1 of the License,
+  or (at your option) any later version.
+
+  PulseAudio is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public License
+  along with PulseAudio; if not, write to the Free Software
+  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
+  USA.
+***/
+
+#include <pulse/sample.h>
+#include <pulse/channelmap.h>
+#include <pulsecore/memchunk.h>
+
+
+typedef struct pa_lfe_filter pa_lfe_filter_t;
+
+pa_lfe_filter_t * pa_lfe_filter_new(const pa_sample_spec* ss, const pa_channel_map* cm, float crossover_freq);
+void pa_lfe_filter_free(pa_lfe_filter_t *);
+void pa_lfe_filter_reset(pa_lfe_filter_t *);
+pa_memchunk * pa_lfe_filter_process(pa_lfe_filter_t *filter, pa_memchunk *buf);
+void pa_lfe_filter_update_rate(pa_lfe_filter_t *, uint32_t new_rate);
+
+#endif
diff --git a/src/pulsecore/resampler.c b/src/pulsecore/resampler.c
index 0d7cc20b8..157671df9 100644
--- a/src/pulsecore/resampler.c
+++ b/src/pulsecore/resampler.c
@@ -40,7 +40,7 @@ struct ffmpeg_data { /* data specific to ffmpeg */
 
 static int copy_init(pa_resampler *r);
 
-static void setup_remap(const pa_resampler *r, pa_remap_t *m);
+static void setup_remap(const pa_resampler *r, pa_remap_t *m, bool *lfe_filter_required);
 static void free_remap(pa_remap_t *m);
 
 static int (* const init_table[])(pa_resampler *r) = {
@@ -324,6 +324,7 @@ pa_resampler* pa_resampler_new(
         pa_resample_flags_t flags) {
 
     pa_resampler *r = NULL;
+    bool lfe_filter_required = false;
 
     pa_assert(pool);
     pa_assert(a);
@@ -412,7 +413,15 @@ pa_resampler* pa_resampler_new(
 
     /* set up the remap structure */
     if (r->map_required)
-        setup_remap(r, &r->remap);
+        setup_remap(r, &r->remap, &lfe_filter_required);
+
+    if (lfe_filter_required) {
+        pa_sample_spec wss = r->o_ss;
+        wss.format = r->work_format;
+        /* TODO: Temporary code that sets crossover freq to 120 Hz. This should be a parameter */
+        r->lfe_filter = pa_lfe_filter_new(&wss, &r->o_cm, 120.0f);
+        pa_log_debug("  lfe filter activated (LR4 type)");
+    }
 
     /* initialize implementation */
     if (init_table[method](r) < 0)
@@ -434,6 +443,9 @@ void pa_resampler_free(pa_resampler *r) {
     else
         pa_xfree(r->impl.data);
 
+    if (r->lfe_filter)
+        pa_lfe_filter_free(r->lfe_filter);
+
     if (r->to_work_format_buf.memblock)
         pa_memblock_unref(r->to_work_format_buf.memblock);
     if (r->remap_buf.memblock)
@@ -472,6 +484,9 @@ void pa_resampler_set_output_rate(pa_resampler *r, uint32_t rate) {
     r->o_ss.rate = rate;
 
     r->impl.update_rates(r);
+
+    if (r->lfe_filter)
+        pa_lfe_filter_update_rate(r->lfe_filter, rate);
 }
 
 size_t pa_resampler_request(pa_resampler *r, size_t out_length) {
@@ -556,6 +571,9 @@ void pa_resampler_reset(pa_resampler *r) {
     if (r->impl.reset)
         r->impl.reset(r);
 
+    if (r->lfe_filter)
+        pa_lfe_filter_reset(r->lfe_filter);
+
     *r->have_leftover = false;
 }
 
@@ -756,7 +774,7 @@ static int front_rear_side(pa_channel_position_t p) {
     return ON_OTHER;
 }
 
-static void setup_remap(const pa_resampler *r, pa_remap_t *m) {
+static void setup_remap(const pa_resampler *r, pa_remap_t *m, bool *lfe_filter_required) {
     unsigned oc, ic;
     unsigned n_oc, n_ic;
     bool ic_connected[PA_CHANNELS_MAX];
@@ -765,6 +783,7 @@ static void setup_remap(const pa_resampler *r, pa_remap_t *m) {
 
     pa_assert(r);
     pa_assert(m);
+    pa_assert(lfe_filter_required);
 
     n_oc = r->o_ss.channels;
     n_ic = r->i_ss.channels;
@@ -777,6 +796,7 @@ static void setup_remap(const pa_resampler *r, pa_remap_t *m) {
     memset(m->map_table_i, 0, sizeof(m->map_table_i));
 
     memset(ic_connected, 0, sizeof(ic_connected));
+    *lfe_filter_required = false;
 
     if (r->flags & PA_RESAMPLER_NO_REMAP) {
         for (oc = 0; oc < PA_MIN(n_ic, n_oc); oc++)
@@ -888,6 +908,9 @@ static void setup_remap(const pa_resampler *r, pa_remap_t *m) {
 
                     oc_connected = true;
                     ic_connected[ic] = true;
+
+                    if (a == PA_CHANNEL_POSITION_MONO && on_lfe(b) && !(r->flags & PA_RESAMPLER_NO_LFE))
+                        *lfe_filter_required = true;
                 }
                 else if (b == PA_CHANNEL_POSITION_MONO) {
                     m->map_table_f[oc][ic] = 1.0f / (float) n_ic;
@@ -970,6 +993,8 @@ static void setup_remap(const pa_resampler *r, pa_remap_t *m) {
 
                     /* Please note that a channel connected to LFE doesn't
                      * really count as connected. */
+
+                    *lfe_filter_required = true;
                 }
             }
         }
@@ -1340,6 +1365,9 @@ void pa_resampler_run(pa_resampler *r, const pa_memchunk *in, pa_memchunk *out)
         buf = remap_channels(r, buf);
     }
 
+    if (r->lfe_filter)
+        buf = pa_lfe_filter_process(r->lfe_filter, buf);
+
     if (buf->length) {
         buf = convert_from_work_format(r, buf);
         *out = *buf;
diff --git a/src/pulsecore/resampler.h b/src/pulsecore/resampler.h
index 4840a5eae..45ad75a57 100644
--- a/src/pulsecore/resampler.h
+++ b/src/pulsecore/resampler.h
@@ -26,6 +26,7 @@
 #include <pulsecore/memchunk.h>
 #include <pulsecore/sconv.h>
 #include <pulsecore/remap.h>
+#include <pulsecore/filter/lfe-filter.h>
 
 typedef struct pa_resampler pa_resampler;
 typedef struct pa_resampler_impl pa_resampler_impl;
@@ -106,6 +107,8 @@ struct pa_resampler {
     pa_remap_t remap;
     bool map_required;
 
+    pa_lfe_filter_t *lfe_filter;
+
     pa_resampler_impl impl;
 };