EQ FIR: HiFiEP optimized version

Signed-off-by: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>

Author: singalsu

src/audio/fir_hifi2ep.c

@@ -0,0 +1,189 @@
+/*
+ * Copyright (c) 2017, Intel Corporation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in the
+ *     documentation and/or other materials provided with the distribution.
+ *   * Neither the name of the Intel Corporation nor the
+ *     names of its contributors may be used to endorse or promote products
+ *     derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>
+ */
+
+#include <stdint.h>
+#include <stddef.h>
+#include <errno.h>
+#include <sof/audio/component.h>
+#include <sof/audio/format.h>
+#include <uapi/eq.h>
+#include "fir_config.h"
+
+#if FIR_HIFIEP
+
+#include <xtensa/config/defs.h>
+#include <xtensa/tie/xt_hifi2.h>
+#include "fir_hifi2ep.h"
+
+/*
+ * EQ FIR algorithm code
+ */
+
+void fir_reset(struct fir_state_32x16 *fir)
+{
+	fir->mute = 1;
+	fir->length = 0;
+	fir->out_shift = 0;
+	fir->rwp = NULL;
+	fir->delay = NULL;
+	fir->delay_end = NULL;
+	fir->coef = NULL;
+	/* There may need to know the beginning of dynamic allocation after
+	 * reset so omitting setting also fir->delay to NULL.
+	 */
+}
+
+int fir_init_coef(struct fir_state_32x16 *fir, int16_t config[])
+{
+	struct sof_eq_fir_coef_data *setup;
+
+	/* The length is taps plus two since the filter computes two
+	 * samples per call. Length plus one would be minimum but the add
+	 * must be even. The even length is needed for 64 bit loads from delay
+	 * lines with 32 bit samples.
+	 */
+	setup = (struct sof_eq_fir_coef_data *)config;
+	fir->mute = 0;
+	fir->rwp = NULL;
+	fir->taps = (int)setup->length;
+	fir->length = fir->taps + 2;
+	fir->out_shift = (int)setup->out_shift;
+	fir->coef = (ae_p16x2s *)&setup->coef[0];
+	fir->delay = NULL;
+	fir->delay_end = NULL;
+
+	/* Check FIR tap count for implementation specific constraints */
+	if (fir->taps > SOF_EQ_FIR_MAX_LENGTH || fir->taps < 4)
+		return -EINVAL;
+
+	if (fir->taps & 3)
+		return -EINVAL;
+
+	return fir->length;
+}
+
+void fir_init_delay(struct fir_state_32x16 *fir, int32_t **data)
+{
+	fir->delay = (ae_p24f *) *data;
+	fir->delay_end = fir->delay + fir->length;
+	fir->rwp = (ae_p24x2f *)(fir->delay + fir->length - 1);
+	*data += fir->length; /* Point to next delay line start */
+}
+
+void fir_get_lrshifts(struct fir_state_32x16 *fir, int *lshift,
+		      int *rshift)
+{
+	if (fir->mute) {
+		*lshift = 0;
+		*rshift = 31;
+	} else {
+		*lshift = (fir->out_shift < 0) ? -fir->out_shift : 0;
+		*rshift = (fir->out_shift > 0) ? fir->out_shift : 0;
+	}
+}
+
+/* For even frame lengths use FIR filter that processes two sequential
+ * sample per call.
+ */
+void eq_fir_2x_s32_hifiep(struct fir_state_32x16 fir[],
+			  struct comp_buffer *source,
+			  struct comp_buffer *sink,
+			  int frames, int nch)
+{
+	struct fir_state_32x16 *f;
+	int32_t *src = (int32_t *)source->r_ptr;
+	int32_t *snk = (int32_t *)sink->w_ptr;
+	int32_t *x0;
+	int32_t *y0;
+	int32_t *x1;
+	int32_t *y1;
+	int ch;
+	int i;
+	int rshift;
+	int lshift;
+	int inc = nch << 1;
+
+	for (ch = 0; ch < nch; ch++) {
+		/* Get FIR instance and get shifts to e.g. apply mute
+		 * without overhead.
+		 */
+		f = &fir[ch];
+		fir_get_lrshifts(f, &lshift, &rshift);
+
+		/* Setup circular buffer for FIR input data delay */
+		fir_hifiep_setup_circular(f);
+
+		x0 = src++;
+		y0 = snk++;
+		for (i = 0; i < (frames >> 1); i++) {
+			x1 = x0 + nch;
+			y1 = y0 + nch;
+			fir_32x16_2x_hifiep(f, x0, x1, y0, y1, lshift, rshift);
+			x0 += inc;
+			y0 += inc;
+		}
+	}
+}
+
+/* FIR for any number of frames */
+void eq_fir_s32_hifiep(struct fir_state_32x16 fir[], struct comp_buffer *source,
+		       struct comp_buffer *sink, int frames, int nch)
+{
+	struct fir_state_32x16 *f;
+	int32_t *src = (int32_t *)source->r_ptr;
+	int32_t *snk = (int32_t *)sink->w_ptr;
+	int32_t *x;
+	int32_t *y;
+	int ch;
+	int i;
+	int rshift;
+	int lshift;
+
+	for (ch = 0; ch < nch; ch++) {
+		/* Get FIR instance and get shifts to e.g. apply mute
+		 * without overhead.
+		 */
+		f = &fir[ch];
+		fir_get_lrshifts(f, &lshift, &rshift);
+
+		/* Setup circular buffer for FIR input data delay */
+		fir_hifiep_setup_circular(f);
+
+		x = src++;
+		y = snk++;
+		for (i = 0; i < frames; i++) {
+			fir_32x16_hifiep(f, x, y, lshift, rshift);
+			x += nch;
+			y += nch;
+		}
+	}
+}
+
+#endif