/* 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
		}
	}
}