src/audio/eq_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>
#include <stdbool.h>
#include <sof/sof.h>
#include <sof/lock.h>
#include <sof/list.h>
#include <sof/stream.h>
#include <sof/alloc.h>
#include <sof/work.h>
#include <sof/clock.h>
#include <sof/audio/component.h>
#include <sof/audio/pipeline.h>
#include <sof/audio/format.h>
#include <uapi/ipc.h>
#include <uapi/eq.h>
#include "eq_iir.h"
#include "iir.h"

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

#define trace_eq_iir(__e) trace_event(TRACE_CLASS_EQ_IIR, __e)
#define tracev_eq_iir(__e) tracev_event(TRACE_CLASS_EQ_IIR, __e)
#define trace_eq_iir_error(__e) trace_error(TRACE_CLASS_EQ_IIR, __e)

/* src component private data */
struct comp_data {
	struct sof_eq_iir_config *config;
	uint32_t period_bytes;
	struct iir_state_df2t iir[PLATFORM_MAX_CHANNELS];
	void (*eq_iir_func)(struct comp_dev *dev,
		struct comp_buffer *source,
		struct comp_buffer *sink,
		uint32_t frames);
};

/*
 * EQ IIR algorithm code
 */

static void eq_iir_s32_default(struct comp_dev *dev,
	struct comp_buffer *source, struct comp_buffer *sink, uint32_t frames)
{
	struct comp_data *cd = comp_get_drvdata(dev);
	int ch;
	int n;
	int n_wrap_src;
	int n_wrap_snk;
	int n_wrap_min;
	int32_t *src = (int32_t *) source->r_ptr;
	int32_t *snk = (int32_t *) sink->w_ptr;
	int nch = dev->params.channels;
	int32_t *x = src + nch - 1;
	int32_t *y = snk + nch - 1;

	for (ch = 0; ch < nch; ch++) {
		n = frames * nch;
		x = src++;
		y = snk++;
		while (n > 0) {
			n_wrap_src = (int32_t *) source->end_addr - x;
			n_wrap_snk = (int32_t *) sink->end_addr - y;
			n_wrap_min = (n_wrap_src < n_wrap_snk) ?
				n_wrap_src : n_wrap_snk;
			if (n < n_wrap_min) {
				/* No circular wrap need */
				while (n > 0) {
					*y = iir_df2t(&cd->iir[ch], *x);
					x += nch;
					y += nch;
					n -= nch;
				}
			} else {
				/* Wrap in n_wrap_min/nch samples */
				while (n_wrap_min > 0) {
					*y = iir_df2t(&cd->iir[ch], *x);
					x += nch;
					y += nch;
					n_wrap_min -= nch;
					n -= nch;
				}
				/* Check both source and destination for wrap */
				if (x > (int32_t *) source->end_addr)
					x = (int32_t *)
					((size_t) x - source->size);
				if (snk > (int32_t *) sink->end_addr)
					y = (int32_t *)
					((size_t) y - sink->size);
			}
		}

	}
}

static void eq_iir_free_parameters(struct sof_eq_iir_config **config)
{
	if (*config != NULL)
		rfree(*config);

	*config = NULL;
}

static void eq_iir_free_delaylines(struct iir_state_df2t *iir)
{
	int i = 0;
	int64_t *delay = NULL; /* TODO should not need to know the type */

	/* 1st active EQ delay line data is at beginning of the single
	 * allocated buffer
	 */
	for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) {
		if ((iir[i].delay != NULL) && (delay == NULL))
			delay = iir[i].delay;

		/* Point all delays to NULL to avoid duplicated free later */
		iir[i].delay = NULL;
	}

	if (delay != NULL)
		rfree(delay);

}

static int eq_iir_setup(struct iir_state_df2t iir[],
	struct sof_eq_iir_config *config, int nch)
{
	int i;
	int j;
	int idx;
	int resp;
	size_t s;
	size_t size_sum = 0;
	int64_t *iir_delay; /* TODO should not need to know the type */
	int32_t *coef_data, *assign_response;
	int response_index[PLATFORM_MAX_CHANNELS];

	/* Free existing IIR channels data if it was allocated */
	eq_iir_free_delaylines(iir);

	if ((nch > PLATFORM_MAX_CHANNELS)
		|| (config->channels_in_config > PLATFORM_MAX_CHANNELS))
		return -EINVAL;

	/* Collect index of response start positions in all_coefficients[]  */
	j = 0;
	assign_response = &config->data[0];
	coef_data = &config->data[config->channels_in_config];
	for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) {
		if (i < config->number_of_responses) {
			response_index[i] = j;
			j += NHEADER_DF2T
				+ NBIQUAD_DF2T * coef_data[j];
		} else {
			response_index[i] = 0;
		}
	}

	/* Initialize 1st phase */
	for (i = 0; i < nch; i++) {
		resp = assign_response[i];
		if (resp > config->number_of_responses - 1)
			return -EINVAL;

		if (resp < 0) {
			/* Initialize EQ channel to bypass */
			iir_reset_df2t(&iir[i]);
		} else {
			/* Initialize EQ coefficients */
			idx = response_index[resp];
			s = iir_init_coef_df2t(&iir[i], &coef_data[idx]);
			if (s > 0)
				size_sum += s;
			else
				return -EINVAL;
		}

	}

	/* Allocate all IIR channels data in a big chunk and clear it */
	iir_delay = rzalloc(RZONE_RUNTIME, SOF_MEM_CAPS_RAM, size_sum);
	if (iir_delay == NULL)
		return -ENOMEM;

	memset(iir_delay, 0, size_sum);

	/* Initialize 2nd phase to set EQ delay lines pointers */
	for (i = 0; i < nch; i++) {
		resp = assign_response[i];
		if (resp >= 0) {
			iir_init_delay_df2t(&iir[i], &iir_delay);
		}

	}

	return 0;
}

static int eq_iir_switch_response(struct iir_state_df2t iir[],
	struct sof_eq_iir_config *config, uint32_t ch, int32_t response)
{
	int ret;

	/* Copy assign response from update and re-initialize EQ */
	if ((config == NULL) || (ch >= PLATFORM_MAX_CHANNELS))
		return -EINVAL;

	config->data[ch] = response;
	ret = eq_iir_setup(iir, config, PLATFORM_MAX_CHANNELS);

	return ret;
}

/*
 * End of EQ setup code. Next the standard component methods.
 */

static struct comp_dev *eq_iir_new(struct sof_ipc_comp *comp)
{
	struct comp_dev *dev;
	struct comp_data *cd;
	int i;

	trace_eq_iir("new");

	dev = rzalloc(RZONE_RUNTIME, SOF_MEM_CAPS_RAM,
		COMP_SIZE(struct sof_ipc_comp_eq_iir));
	if (dev == NULL)
		return NULL;

	memcpy(&dev->comp, comp, sizeof(struct sof_ipc_comp_eq_iir));

	cd = rzalloc(RZONE_RUNTIME, SOF_MEM_CAPS_RAM, sizeof(*cd));
	if (cd == NULL) {
		rfree(dev);
		return NULL;
	}

	comp_set_drvdata(dev, cd);

	cd->eq_iir_func = eq_iir_s32_default;
	cd->config = NULL;
	for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)
		iir_reset_df2t(&cd->iir[i]);

	dev->state = COMP_STATE_READY;
	return dev;
}

static void eq_iir_free(struct comp_dev *dev)
{
	struct comp_data *cd = comp_get_drvdata(dev);

	trace_eq_iir("fre");

	eq_iir_free_delaylines(cd->iir);
	eq_iir_free_parameters(&cd->config);

	rfree(cd);
	rfree(dev);
}

/* set component audio stream parameters */
static int eq_iir_params(struct comp_dev *dev)
{
	struct comp_data *cd = comp_get_drvdata(dev);
	struct sof_ipc_comp_config *config = COMP_GET_CONFIG(dev);
	struct comp_buffer *sink;
	int err;

	trace_eq_iir("par");

	/* Calculate period size based on config. First make sure that
	 * frame_bytes is set.
	 */
	dev->frame_bytes =
		dev->params.sample_container_bytes * dev->params.channels;
	cd->period_bytes = dev->frames * dev->frame_bytes;

	/* configure downstream buffer */
	sink = list_first_item(&dev->bsink_list, struct comp_buffer, source_list);
	err = buffer_set_size(sink, cd->period_bytes * config->periods_sink);
	if (err < 0) {
		trace_eq_iir_error("eSz");
		return err;
	}

	/* EQ supports only S32_LE PCM format */
	if (config->frame_fmt != SOF_IPC_FRAME_S32_LE)
		return -EINVAL;

	return 0;
}

static int iir_cmd_set_value(struct comp_dev *dev, struct sof_ipc_ctrl_data *cdata)
{
	struct comp_data *cd = comp_get_drvdata(dev);
	int j;
	uint32_t ch;
	bool val;

	if (cdata->cmd == SOF_CTRL_CMD_SWITCH) {
		trace_eq_iir("mst");
		for (j = 0; j < cdata->num_elems; j++) {
			ch = cdata->chanv[j].channel;
			val = cdata->chanv[j].value;
			tracev_value(ch);
			tracev_value(val);
			if (ch >= PLATFORM_MAX_CHANNELS) {
				trace_eq_iir_error("che");
				return -EINVAL;
			}
			if (val)
				iir_unmute_df2t(&cd->iir[ch]);
			else
				iir_mute_df2t(&cd->iir[ch]);
		}
	} else {
		trace_eq_iir_error("ste");
		return -EINVAL;
	}

	return 0;
}

static int iir_cmd_set_data(struct comp_dev *dev, struct sof_ipc_ctrl_data *cdata)
{
	struct comp_data *cd = comp_get_drvdata(dev);
	struct sof_ipc_ctrl_value_comp *compv;
	int i;
	int ret = 0;
	size_t bs;

	switch (cdata->cmd) {
	case SOF_CTRL_CMD_ENUM:
		trace_eq_iir("EIe");
		if (cdata->index == SOF_EQ_IIR_IDX_SWITCH) {
			trace_eq_iir("EIs");
			compv = (struct sof_ipc_ctrl_value_comp *) cdata->data->data;
			for (i = 0; i < (int) cdata->num_elems; i++) {
				tracev_value(compv[i].index);
				tracev_value(compv[i].svalue);
				ret = eq_iir_switch_response(cd->iir, cd->config,
					compv[i].index, compv[i].svalue);
				if (ret < 0) {
					trace_eq_iir_error("swe");
					return -EINVAL;
				}
			}
		} else {
			trace_eq_iir_error("une");
			trace_error_value(cdata->index);
			return -EINVAL;
		}
		break;
	case SOF_CTRL_CMD_BINARY:
		trace_eq_iir("EIb");
		/* Check and free old config */
		eq_iir_free_parameters(&cd->config);

		/* Copy new config, need to decode data to know the size */
		bs = cdata->data->size;
		if ((bs > SOF_EQ_IIR_MAX_SIZE) || (bs < 1))
			return -EINVAL;

		/* Allocate and make a copy of the blob and setup IIR */
		cd->config = rzalloc(RZONE_RUNTIME, SOF_MEM_CAPS_RAM, bs);
		if (cd->config == NULL)
			return -EINVAL;

		memcpy(cd->config, cdata->data->data, bs);
		/* Initialize all channels, the actual number of channels may
		 * not be set yet.
		 */
		ret = eq_iir_setup(cd->iir, cd->config, PLATFORM_MAX_CHANNELS);
		break;
	default:
		trace_eq_iir_error("ec1");
		ret = -EINVAL;
		break;
	}

	return ret;
}

/* used to pass standard and bespoke commands (with data) to component */
static int eq_iir_cmd(struct comp_dev *dev, int cmd, void *data)
{
	struct sof_ipc_ctrl_data *cdata = data;
	int ret = 0;

	trace_eq_iir("cmd");

	switch (cmd) {
	case COMP_CMD_SET_VALUE:
		ret = iir_cmd_set_value(dev, cdata);
		break;
	case COMP_CMD_SET_DATA:
		ret = iir_cmd_set_data(dev, cdata);
		break;
	}

	return ret;
}

static int eq_iir_trigger(struct comp_dev *dev, int cmd)
{
	trace_eq_iir("trg");

	return comp_set_state(dev, cmd);
}

/* copy and process stream data from source to sink buffers */
static int eq_iir_copy(struct comp_dev *dev)
{
	struct comp_data *cd = comp_get_drvdata(dev);
	struct comp_buffer *source;
	struct comp_buffer *sink;
	int res;

	trace_comp("EqI");

	/* get source and sink buffers */
	source = list_first_item(&dev->bsource_list, struct comp_buffer,
		sink_list);
	sink = list_first_item(&dev->bsink_list, struct comp_buffer,
		source_list);

	/* make sure source component buffer has enough data available and that
	 * the sink component buffer has enough free bytes for copy. Also
	 * check for XRUNs */
	res = comp_buffer_can_copy_bytes(source, sink, cd->period_bytes);
	if (res) {
		trace_eq_iir_error("xrn");
		return -EIO;	/* xrun */
	}

	cd->eq_iir_func(dev, source, sink, dev->frames);

	/* calc new free and available */
	comp_update_buffer_consume(source, cd->period_bytes);
	comp_update_buffer_produce(sink, cd->period_bytes);

	return dev->frames;
}

static int eq_iir_prepare(struct comp_dev *dev)
{
	struct comp_data *cd = comp_get_drvdata(dev);
	int ret;

	trace_eq_iir("EPp");

	ret = comp_set_state(dev, COMP_TRIGGER_PREPARE);
	if (ret < 0)
		return ret;

	cd->eq_iir_func = eq_iir_s32_default;

	/* Initialize EQ. Note that if EQ has not received command to
	 * configure the response the EQ prepare returns an error that
	 * interrupts pipeline prepare for downstream.
	 */
	if (cd->config == NULL) {
		comp_set_state(dev, COMP_TRIGGER_RESET);
		return -EINVAL;
	}

	ret = eq_iir_setup(cd->iir, cd->config, dev->params.channels);
	if (ret < 0) {
		comp_set_state(dev, COMP_TRIGGER_RESET);
		return ret;
	}

	return 0;
}

static int eq_iir_reset(struct comp_dev *dev)
{
	int i;
	struct comp_data *cd = comp_get_drvdata(dev);

	trace_eq_iir("ERe");

	eq_iir_free_delaylines(cd->iir);
	eq_iir_free_parameters(&cd->config);

	cd->eq_iir_func = eq_iir_s32_default;
	for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)
		iir_reset_df2t(&cd->iir[i]);

	comp_set_state(dev, COMP_TRIGGER_RESET);
	return 0;
}

struct comp_driver comp_eq_iir = {
	.type = SOF_COMP_EQ_IIR,
	.ops = {
		.new = eq_iir_new,
		.free = eq_iir_free,
		.params = eq_iir_params,
		.cmd = eq_iir_cmd,
		.trigger = eq_iir_trigger,
		.copy = eq_iir_copy,
		.prepare = eq_iir_prepare,
		.reset = eq_iir_reset,
	},
};

void sys_comp_eq_iir_init(void)
{
	comp_register(&comp_eq_iir);
}