/* 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.
*/
#include <stdlib.h>
#include "eq2.h"
struct eq2 {
int n[2];
struct biquad biquad[MAX_BIQUADS_PER_EQ2][2];
};
struct eq2 *eq2_new()
{
struct eq2 *eq2 = (struct eq2 *)calloc(1, sizeof(*eq2));
int i, j;
/* Initialize all biquads to identity filter, so if two channels have
* different numbers of biquads, it still works. */
for (i = 0; i < MAX_BIQUADS_PER_EQ2; i++)
for (j = 0; j < 2; j++)
biquad_set(&eq2->biquad[i][j], BQ_NONE, 0, 0, 0);
return eq2;
}
void eq2_free(struct eq2 *eq2)
{
free(eq2);
}
int eq2_append_biquad(struct eq2 *eq2, int channel,
enum biquad_type type, float freq, float Q, float gain)
{
if (eq2->n[channel] >= MAX_BIQUADS_PER_EQ2)
return -1;
biquad_set(&eq2->biquad[eq2->n[channel]++][channel], type, freq, Q,
gain);
return 0;
}
int eq2_append_biquad_direct(struct eq2 *eq2, int channel,
const struct biquad *biquad)
{
if (eq2->n[channel] >= MAX_BIQUADS_PER_EQ2)
return -1;
eq2->biquad[eq2->n[channel]++][channel] = *biquad;
return 0;
}
static inline void eq2_process_one(struct biquad (*bq)[2],
float *data0, float *data1, int count)
{
struct biquad *qL = &bq[0][0];
struct biquad *qR = &bq[0][1];
float x1L = qL->x1;
float x2L = qL->x2;
float y1L = qL->y1;
float y2L = qL->y2;
float b0L = qL->b0;
float b1L = qL->b1;
float b2L = qL->b2;
float a1L = qL->a1;
float a2L = qL->a2;
float x1R = qR->x1;
float x2R = qR->x2;
float y1R = qR->y1;
float y2R = qR->y2;
float b0R = qR->b0;
float b1R = qR->b1;
float b2R = qR->b2;
float a1R = qR->a1;
float a2R = qR->a2;
int j;
for (j = 0; j < count; j++) {
float xL = data0[j];
float xR = data1[j];
float yL = b0L*xL
+ b1L*x1L + b2L*x2L
- a1L*y1L - a2L*y2L;
x2L = x1L;
x1L = xL;
y2L = y1L;
y1L = yL;
float yR = b0R*xR
+ b1R*x1R + b2R*x2R
- a1R*y1R - a2R*y2R;
x2R = x1R;
x1R = xR;
y2R = y1R;
y1R = yR;
data0[j] = yL;
data1[j] = yR;
}
qL->x1 = x1L;
qL->x2 = x2L;
qL->y1 = y1L;
qL->y2 = y2L;
qR->x1 = x1R;
qR->x2 = x2R;
qR->y1 = y1R;
qR->y2 = y2R;
}
#ifdef __ARM_NEON__
#include <arm_neon.h>
static inline void eq2_process_two_neon(struct biquad (*bq)[2],
float *data0, float *data1, int count)
{
struct biquad *qL = &bq[0][0];
struct biquad *rL = &bq[1][0];
struct biquad *qR = &bq[0][1];
struct biquad *rR = &bq[1][1];
float32x2_t x1 = {qL->x1, qR->x1};
float32x2_t x2 = {qL->x2, qR->x2};
float32x2_t y1 = {qL->y1, qR->y1};
float32x2_t y2 = {qL->y2, qR->y2};
float32x2_t qb0 = {qL->b0, qR->b0};
float32x2_t qb1 = {qL->b1, qR->b1};
float32x2_t qb2 = {qL->b2, qR->b2};
float32x2_t qa1 = {qL->a1, qR->a1};
float32x2_t qa2 = {qL->a2, qR->a2};
float32x2_t z1 = {rL->y1, rR->y1};
float32x2_t z2 = {rL->y2, rR->y2};
float32x2_t rb0 = {rL->b0, rR->b0};
float32x2_t rb1 = {rL->b1, rR->b1};
float32x2_t rb2 = {rL->b2, rR->b2};
float32x2_t ra1 = {rL->a1, rR->a1};
float32x2_t ra2 = {rL->a2, rR->a2};
__asm__ __volatile__(
/* d0 = x, d1 = y, d2 = z */
"1: \n"
"vmul.f32 d1, %P[qb1], %P[x1] \n"
"vld1.32 d0[0], [%[data0]] \n"
"vld1.32 d0[1], [%[data1]] \n"
"subs %[count], #1 \n"
"vmul.f32 d2, %P[rb1], %P[y1] \n"
"vmla.f32 d1, %P[qb0], d0 \n"
"vmla.f32 d1, %P[qb2], %P[x2] \n"
"vmov.f32 %P[x2], %P[x1] \n"
"vmov.f32 %P[x1], d0 \n"
"vmls.f32 d1, %P[qa1], %P[y1] \n"
"vmls.f32 d1, %P[qa2], %P[y2] \n"
"vmla.f32 d2, %P[rb0], d1 \n"
"vmla.f32 d2, %P[rb2], %P[y2] \n"
"vmov.f32 %P[y2], %P[y1] \n"
"vmov.f32 %P[y1], d1 \n"
"vmls.f32 d2, %P[ra1], %P[z1] \n"
"vmls.f32 d2, %P[ra2], %P[z2] \n"
"vmov.f32 %P[z2], %P[z1] \n"
"vmov.f32 %P[z1], d2 \n"
"vst1.f32 d2[0], [%[data0]]! \n"
"vst1.f32 d2[1], [%[data1]]! \n"
"bne 1b \n"
: /* output */
[data0]"+r"(data0),
[data1]"+r"(data1),
[count]"+r"(count),
[x1]"+w"(x1),
[x2]"+w"(x2),
[y1]"+w"(y1),
[y2]"+w"(y2),
[z1]"+w"(z1),
[z2]"+w"(z2)
: /* input */
[qb0]"w"(qb0),
[qb1]"w"(qb1),
[qb2]"w"(qb2),
[qa1]"w"(qa1),
[qa2]"w"(qa2),
[rb0]"w"(rb0),
[rb1]"w"(rb1),
[rb2]"w"(rb2),
[ra1]"w"(ra1),
[ra2]"w"(ra2)
: /* clobber */
"d0", "d1", "d2", "memory", "cc"
);
qL->x1 = x1[0];
qL->x2 = x2[0];
qL->y1 = y1[0];
qL->y2 = y2[0];
rL->y1 = z1[0];
rL->y2 = z2[0];
qR->x1 = x1[1];
qR->x2 = x2[1];
qR->y1 = y1[1];
qR->y2 = y2[1];
rR->y1 = z1[1];
rR->y2 = z2[1];
}
#endif
#if defined(__SSE3__) && defined(__x86_64__)
#include <emmintrin.h>
static inline void eq2_process_two_sse3(struct biquad (*bq)[2],
float *data0, float *data1, int count)
{
struct biquad *qL = &bq[0][0];
struct biquad *rL = &bq[1][0];
struct biquad *qR = &bq[0][1];
struct biquad *rR = &bq[1][1];
__m128 x1 = {qL->x1, qR->x1};
__m128 x2 = {qL->x2, qR->x2};
__m128 y1 = {qL->y1, qR->y1};
__m128 y2 = {qL->y2, qR->y2};
__m128 qb0 = {qL->b0, qR->b0};
__m128 qb1 = {qL->b1, qR->b1};
__m128 qb2 = {qL->b2, qR->b2};
__m128 qa1 = {qL->a1, qR->a1};
__m128 qa2 = {qL->a2, qR->a2};
__m128 z1 = {rL->y1, rR->y1};
__m128 z2 = {rL->y2, rR->y2};
__m128 rb0 = {rL->b0, rR->b0};
__m128 rb1 = {rL->b1, rR->b1};
__m128 rb2 = {rL->b2, rR->b2};
__m128 ra1 = {rL->a1, rR->a1};
__m128 ra2 = {rL->a2, rR->a2};
__asm__ __volatile__(
"1: \n"
"movss (%[data0]), %%xmm2 \n"
"movss (%[data1]), %%xmm1 \n"
"unpcklps %%xmm1, %%xmm2 \n"
"mulps %[qb2],%[x2] \n"
"lddqu %[qb0],%%xmm0 \n"
"mulps %[ra2],%[z2] \n"
"lddqu %[qb1],%%xmm1 \n"
"mulps %%xmm2,%%xmm0 \n"
"mulps %[x1],%%xmm1 \n"
"addps %%xmm1,%%xmm0 \n"
"movaps %[qa1],%%xmm1 \n"
"mulps %[y1],%%xmm1 \n"
"addps %[x2],%%xmm0 \n"
"movaps %[rb1],%[x2] \n"
"mulps %[y1],%[x2] \n"
"subps %%xmm1,%%xmm0 \n"
"movaps %[qa2],%%xmm1 \n"
"mulps %[y2],%%xmm1 \n"
"mulps %[rb2],%[y2] \n"
"subps %%xmm1,%%xmm0 \n"
"movaps %[rb0],%%xmm1 \n"
"mulps %%xmm0,%%xmm1 \n"
"addps %[x2],%%xmm1 \n"
"movaps %[x1],%[x2] \n"
"movaps %%xmm2,%[x1] \n"
"addps %[y2],%%xmm1 \n"
"movaps %[ra1],%[y2] \n"
"mulps %[z1],%[y2] \n"
"subps %[y2],%%xmm1 \n"
"movaps %[y1],%[y2] \n"
"movaps %%xmm0,%[y1] \n"
"subps %[z2],%%xmm1 \n"
"movaps %[z1],%[z2] \n"
"movaps %%xmm1,%[z1] \n"
"movss %%xmm1, (%[data0]) \n"
"shufps $1, %%xmm1, %%xmm1 \n"
"movss %%xmm1, (%[data1]) \n"
"add $4, %[data0] \n"
"add $4, %[data1] \n"
"sub $1, %[count] \n"
"jnz 1b \n"
: /* output */
[data0]"+r"(data0),
[data1]"+r"(data1),
[count]"+r"(count),
[x1]"+x"(x1),
[x2]"+x"(x2),
[y1]"+x"(y1),
[y2]"+x"(y2),
[z1]"+x"(z1),
[z2]"+x"(z2)
: /* input */
[qb0]"m"(qb0),
[qb1]"m"(qb1),
[qb2]"m"(qb2),
[qa1]"x"(qa1),
[qa2]"x"(qa2),
[rb0]"x"(rb0),
[rb1]"x"(rb1),
[rb2]"x"(rb2),
[ra1]"x"(ra1),
[ra2]"x"(ra2)
: /* clobber */
"xmm0", "xmm1", "xmm2", "memory", "cc"
);
qL->x1 = x1[0];
qL->x2 = x2[0];
qL->y1 = y1[0];
qL->y2 = y2[0];
rL->y1 = z1[0];
rL->y2 = z2[0];
qR->x1 = x1[1];
qR->x2 = x2[1];
qR->y1 = y1[1];
qR->y2 = y2[1];
rR->y1 = z1[1];
rR->y2 = z2[1];
}
#endif
void eq2_process(struct eq2 *eq2, float *data0, float *data1, int count)
{
int i;
int n;
if (!count)
return;
n = eq2->n[0];
if (eq2->n[1] > n)
n = eq2->n[1];
for (i = 0; i < n; i += 2) {
if (i + 1 == n) {
eq2_process_one(&eq2->biquad[i], data0, data1, count);
} else {
#if defined(__ARM_NEON__)
eq2_process_two_neon(&eq2->biquad[i], data0, data1,
count);
#elif defined(__SSE3__) && defined(__x86_64__)
eq2_process_two_sse3(&eq2->biquad[i], data0, data1,
count);
#else
eq2_process_one(&eq2->biquad[i], data0, data1, count);
eq2_process_one(&eq2->biquad[i+1], data0, data1, count);
#endif
}
}
}