src/audio/iir.c

/*
 * 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>
 *         Liam Girdwood <liam.r.girdwood@linux.intel.com>
 *         Keyon Jie <yang.jie@linux.intel.com>
 */

#include <stdint.h>
#include <stddef.h>
#include <errno.h>

#ifdef MODULE_TEST
#include <stdio.h>
#endif

#include <sof/audio/format.h>
#include "iir.h"

/*
 * Direct form II transposed second order filter block (biquad)
 *
 *              +----+                         +---+    +-------+
 * X(z) ---o--->| b0 |---> + -------------o--->| g |--->| shift |---> Y(z)
 *         |    +----+     ^              |    +---+    +-------+
 *         |               |              |
 *         |            +------+          |
 *         |            | z^-1 |          |
 *         |            +------+          |
 *         |               ^              |
 *         |    +----+     |     +----+   |
 *         o--->| b1 |---> + <---| a1 |---o
 *         |    +----+     ^     +----+   |
 *         |               |              |
 *         |            +------+          |
 *         |            | z^-1 |          |
 *         |            +------+          |
 *         |               ^              |
 *         |    +----+     |     +----+   |
 *         o--->| b2 |---> + <---| a2 |---+
 *              +----+           +----+
 *
 */

/* Series DF2T IIR */

/* 32 bit data, 32 bit coefficients and 64 bit state variables */

int32_t iir_df2t(struct iir_state_df2t *iir, int32_t x)
{
	int32_t in;
	int32_t tmp;
	int64_t acc;
	int32_t out = 0;
	int i;
	int j;
	int d = 0; /* Index to delays */
	int c = 2; /* Index to coefficient a2 */

	/* Coefficients order in coef[] is {a2, a1, b2, b1, b0, shift, gain} */
	in = x;
	for (j = 0; j < iir->biquads; j += iir->biquads_in_series) {
		for (i = 0; i < iir->biquads_in_series; i++) {
			/* Compute output: Delay is Q3.61
			 * Q2.30 x Q1.31 -> Q3.61
			 * Shift Q3.61 to Q3.31 with rounding
			 */
			acc = ((int64_t) iir->coef[c + 4]) * in + iir->delay[d];
			tmp = (int32_t) Q_SHIFT_RND(acc, 61, 31);

			/* Compute 1st delay */
			acc = iir->delay[d + 1];
			acc += ((int64_t) iir->coef[c + 3]) * in; /* Coef  b1 */
			acc += ((int64_t) iir->coef[c + 1]) * tmp; /* Coef a1 */
			iir->delay[d] = acc;

			/* Compute 2nd delay */
			acc = ((int64_t) iir->coef[c + 2]) * in; /* Coef  b2 */
			acc += ((int64_t) iir->coef[c]) * tmp; /* Coef a2 */
			iir->delay[d + 1] = acc;

			/* Gain, output shift, prepare for next biquad
			 * Q2.14 x Q1.31 -> Q3.45, shift too Q3.31 and saturate
			 */
			acc = ((int64_t) iir->coef[c + 6]) * tmp; /* Gain */
			acc = Q_SHIFT_RND(acc, 45 + iir->coef[c + 5], 31);
			in = sat_int32(acc);
			c += 7; /* Next coefficients section */
			d += 2; /* Next biquad delays */
		}
		/* Output of previous section is in variable in */
		out = sat_int32((int64_t) out + in);
	}
	return out;
}

size_t iir_init_coef_df2t(struct iir_state_df2t *iir, int32_t config[])
{
	iir->mute = 0;
	iir->biquads = (int) config[0];
	iir->biquads_in_series = (int) config[1];
	iir->coef = &config[0]; /* TODO: Could change this to config[2] */
	iir->delay = NULL;

	if ((iir->biquads > IIR_DF2T_BIQUADS_MAX) || (iir->biquads < 1)) {
		iir_reset_df2t(iir);
		return -EINVAL;
	}

	return 2 * iir->biquads * sizeof(int64_t); /* Needed delay line size */
}

void iir_init_delay_df2t(struct iir_state_df2t *iir, int64_t **delay)
{
	iir->delay = *delay; /* Delay line of this IIR */
	*delay += 2 * iir->biquads; /* Point to next IIR delay line start */

}

void iir_mute_df2t(struct iir_state_df2t *iir)
{
	iir->mute = 1;
}

void iir_unmute_df2t(struct iir_state_df2t *iir)
{
	iir->mute = 0;
}

void iir_reset_df2t(struct iir_state_df2t *iir)
{
	iir->mute = 1;
	iir->biquads = 0;
	iir->biquads_in_series = 0;
	iir->coef = NULL;
	/* Note: May need to know the beginning of dynamic allocation after so
	 * omitting setting iir->delay to NULL.
	 */
}