diff --git a/spa/plugins/audioconvert/biquad.c b/spa/plugins/audioconvert/biquad.c new file mode 100644 index 000000000..c8b04e66f --- /dev/null +++ b/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 +#endif + +#include + +#include +#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; + } +} diff --git a/spa/plugins/audioconvert/biquad.h b/spa/plugins/audioconvert/biquad.h new file mode 100644 index 000000000..bb8f2fb91 --- /dev/null +++ b/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_ */ diff --git a/spa/plugins/audioconvert/channelmix-ops-c.c b/spa/plugins/audioconvert/channelmix-ops-c.c index f166cf4e6..588cff198 100644 --- a/spa/plugins/audioconvert/channelmix-ops-c.c +++ b/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); } } diff --git a/spa/plugins/audioconvert/channelmix-ops.c b/spa/plugins/audioconvert/channelmix-ops.c index 5640d4ceb..64c904f44 100644 --- a/spa/plugins/audioconvert/channelmix-ops.c +++ b/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++; diff --git a/spa/plugins/audioconvert/channelmix-ops.h b/spa/plugins/audioconvert/channelmix-ops.h index a669d2206..3dd600e4d 100644 --- a/spa/plugins/audioconvert/channelmix-ops.h +++ b/spa/plugins/audioconvert/channelmix-ops.h @@ -28,6 +28,8 @@ #include #include +#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); diff --git a/spa/plugins/audioconvert/crossover.c b/spa/plugins/audioconvert/crossover.c new file mode 100644 index 000000000..19e1ef9f3 --- /dev/null +++ b/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 +#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; +} diff --git a/spa/plugins/audioconvert/crossover.h b/spa/plugins/audioconvert/crossover.h new file mode 100644 index 000000000..f18695ddd --- /dev/null +++ b/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_ */ diff --git a/spa/plugins/audioconvert/meson.build b/spa/plugins/audioconvert/meson.build index 261f11cde..562805f52 100644 --- a/spa/plugins/audioconvert/meson.build +++ b/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',