src/audio/src.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 <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 <uapi/ipc.h>

#include "src_config.h"
#include "src.h"

#if SRC_SHORT
#include <sof/audio/coefficients/src/src_tiny_int16_define.h>
#include <sof/audio/coefficients/src/src_tiny_int16_table.h>
#else
#include <sof/audio/coefficients/src/src_std_int32_define.h>
#include <sof/audio/coefficients/src/src_std_int32_table.h>
#endif

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

#define trace_src(__e) trace_event(TRACE_CLASS_SRC, __e)
#define tracev_src(__e) tracev_event(TRACE_CLASS_SRC, __e)
#define trace_src_error(__e) trace_error(TRACE_CLASS_SRC, __e)

/* The FIR maximum lengths are per channel so need to multiply them */
#define MAX_FIR_DELAY_SIZE_XNCH (PLATFORM_MAX_CHANNELS * MAX_FIR_DELAY_SIZE)
#define MAX_OUT_DELAY_SIZE_XNCH (PLATFORM_MAX_CHANNELS * MAX_OUT_DELAY_SIZE)

/* src component private data */
struct comp_data {
	struct polyphase_src src;
	struct src_param param;
	int32_t *delay_lines;
	uint32_t sink_rate;
	uint32_t source_rate;
	int32_t *sbuf_w_ptr;
	int32_t *sbuf_r_ptr;
	int sbuf_avail;
	void (*src_func)(struct comp_dev *dev,
		struct comp_buffer *source,
		struct comp_buffer *sink,
		size_t *consumed,
		size_t *produced);
	void (*polyphase_func)(struct src_stage_prm *s);
};

/* Calculate ceil() for integer division */
int src_ceil_divide(int a, int b)
{
	int c;

	c = a / b;
	if (c * b < a)
		c++;

	return c;
}

/* Calculates the needed FIR delay line length */
static int src_fir_delay_length(struct src_stage *s)
{
	return s->subfilter_length + (s->num_of_subfilters - 1) * s->idm
		+ s->blk_in;
}

/* Calculates the FIR output delay line length */
static int src_out_delay_length(struct src_stage *s)
{
	return 1 + (s->num_of_subfilters - 1) * s->odm;
}

/* Returns index of a matching sample rate */
static int src_find_fs(int fs_list[], int list_length, int fs)
{
	int i;

	for (i = 0; i < list_length; i++) {
		if (fs_list[i] == fs)
			return i;
	}
	return -EINVAL;
}

/* Calculates buffers to allocate for a SRC mode */
int src_buffer_lengths(struct src_param *a, int fs_in, int fs_out, int nch,
	int frames, int frames_is_for_source)
{
	struct src_stage *stage1;
	struct src_stage *stage2;
	int q;
	int den;
	int num;
	int frames2;

	if (nch > PLATFORM_MAX_CHANNELS) {
		trace_src_error("che");
		tracev_value(nch);
		return -EINVAL;
	}

	a->nch = nch;
	a->idx_in = src_find_fs(src_in_fs, NUM_IN_FS, fs_in);
	a->idx_out = src_find_fs(src_out_fs, NUM_OUT_FS, fs_out);

	/* Check that both in and out rates are supported */
	if (a->idx_in < 0 || a->idx_out < 0) {
		trace_src_error("us1");
		tracev_value(fs_in);
		tracev_value(fs_out);
		return -EINVAL;
	}

	stage1 = src_table1[a->idx_out][a->idx_in];
	stage2 = src_table2[a->idx_out][a->idx_in];

	/* Check from stage1 parameter for a deleted in/out rate combination.*/
	if (stage1->filter_length < 1) {
		trace_src_error("us2");
		tracev_value(fs_in);
		tracev_value(fs_out);
		return -EINVAL;
	}

	a->fir_s1 = nch * src_fir_delay_length(stage1);
	a->out_s1 = nch * src_out_delay_length(stage1);

	/* Find out how many additional times the SRC can be executed
	 * while having block size less or equal to max_frames.
	 */
	if (frames_is_for_source) {
		/* Times that stage1 needs to run to input length of frames */
		a->stage1_times_max = src_ceil_divide(frames, stage1->blk_in);
		q = frames / stage1->blk_in;
		a->stage1_times = MAX(q, 1);
		a->blk_in = a->stage1_times * stage1->blk_in;

		/* Times that stage2 needs to run */
		den = stage2->blk_in * stage1->blk_in;
		num = frames * stage2->blk_out * stage1->blk_out;
		frames2 = src_ceil_divide(num, den);
		a->stage2_times_max = src_ceil_divide(frames2, stage2->blk_out);
		q = frames2 / stage2->blk_out;
		a->stage2_times = MAX(q, 1);
		a->blk_out = a->stage2_times * stage2->blk_out;
	} else {
		/* Times that stage2 needs to run to output length of frames */
		a->stage2_times_max = src_ceil_divide(frames, stage2->blk_out);
		q = frames / stage2->blk_out;
		a->stage2_times = MAX(q, 1);
		a->blk_out = a->stage2_times * stage2->blk_out;

		/* Times that stage1 needs to run */
		num = frames * stage2->blk_in * stage1->blk_in;
		den = stage2->blk_out * stage1->blk_out;
		frames2 = src_ceil_divide(num, den);
		a->stage1_times_max = src_ceil_divide(frames2, stage1->blk_in);
		q = frames2 / stage1->blk_in;
		a->stage1_times = MAX(q, 1);
		a->blk_in = a->stage1_times * stage1->blk_in;
	}

	if (stage2->filter_length == 1) {
		a->fir_s2 = 0;
		a->out_s2 = 0;
		a->stage2_times = 0;
		a->stage2_times_max = 0;
		a->sbuf_length = 0;
	} else {
		a->fir_s2 = nch * src_fir_delay_length(stage2);
		a->out_s2 = nch * src_out_delay_length(stage2);
		/* 2x is an empirically tested length. Since the sink buffer
		 * capability to receive samples varies a shorter stage 2 output
		 * block will create a peak in internal buffer usage.
		 */

		/* TODO 1: Equation for needed length */
		a->sbuf_length = 2 * nch * stage1->blk_out
			* a->stage1_times_max;
	}

	a->src_multich = a->fir_s1 + a->fir_s2 + a->out_s1 + a->out_s2;
	a->total = a->sbuf_length + a->src_multich;

	return 0;
}

static void src_state_reset(struct src_state *state)
{
	state->fir_delay_size = 0;
	state->out_delay_size = 0;
}

static int init_stages(struct src_stage *stage1, struct src_stage *stage2,
	struct polyphase_src *src, struct src_param *p,
	int n, int32_t *delay_lines_start)
{
	/* Clear FIR state */
	src_state_reset(&src->state1);
	src_state_reset(&src->state2);

	src->number_of_stages = n;
	src->stage1 = stage1;
	src->stage2 = stage2;
	if (n == 1 && stage1->blk_out == 0)
		return -EINVAL;

	/* Optimized SRC requires subfilter length multiple of 4 */
	if (stage1->filter_length > 1 && (stage1->subfilter_length & 0x3) > 0)
		return -EINVAL;

	if (stage2->filter_length > 1 && (stage2->subfilter_length & 0x3) > 0)
		return -EINVAL;

	/* Delay line sizes */
	src->state1.fir_delay_size = p->fir_s1;
	src->state1.out_delay_size = p->out_s1;
	src->state1.fir_delay = delay_lines_start;
	src->state1.out_delay =
		src->state1.fir_delay + src->state1.fir_delay_size;
	/* Initialize to last ensures that circular wrap cannot happen
	 * mid-frame. The size is multiple of channels count.
	 */
	src->state1.fir_wp = &src->state1.fir_delay[p->fir_s1 - 1];
	src->state1.out_rp = src->state1.out_delay;
	if (n > 1) {
		src->state2.fir_delay_size = p->fir_s2;
		src->state2.out_delay_size = p->out_s2;
		src->state2.fir_delay =
			src->state1.out_delay + src->state1.out_delay_size;
		src->state2.out_delay =
			src->state2.fir_delay + src->state2.fir_delay_size;
		/* Initialize to last ensures that circular wrap cannot happen
		 * mid-frame. The size is multiple of channels count.
		 */
		src->state2.fir_wp = &src->state2.fir_delay[p->fir_s2 - 1];
		src->state2.out_rp = src->state2.out_delay;
	} else {
		src->state2.fir_delay_size = 0;
		src->state2.out_delay_size = 0;
		src->state2.fir_delay = NULL;
		src->state2.out_delay = NULL;
	}

	/* Check the sizes are less than MAX */
	if (src->state1.fir_delay_size > MAX_FIR_DELAY_SIZE_XNCH ||
		src->state1.out_delay_size > MAX_OUT_DELAY_SIZE_XNCH ||
		src->state2.fir_delay_size > MAX_FIR_DELAY_SIZE_XNCH ||
		src->state2.out_delay_size > MAX_OUT_DELAY_SIZE_XNCH) {
		src->state1.fir_delay = NULL;
		src->state1.out_delay = NULL;
		src->state2.fir_delay = NULL;
		src->state2.out_delay = NULL;
		return -EINVAL;
	}

	return 0;
}

void src_polyphase_reset(struct polyphase_src *src)
{
	src->number_of_stages = 0;
	src->stage1 = NULL;
	src->stage2 = NULL;
	src_state_reset(&src->state1);
	src_state_reset(&src->state2);
}

int src_polyphase_init(struct polyphase_src *src, struct src_param *p,
	int32_t *delay_lines_start)
{
	struct src_stage *stage1;
	struct src_stage *stage2;
	int n_stages;
	int ret;

	if (p->idx_in < 0 || p->idx_out < 0)
		return -EINVAL;

	/* Get setup for 2 stage conversion */
	stage1 = src_table1[p->idx_out][p->idx_in];
	stage2 = src_table2[p->idx_out][p->idx_in];
	ret = init_stages(stage1, stage2, src, p, 2, delay_lines_start);
	if (ret < 0)
		return -EINVAL;

	/* Get number of stages used for optimize opportunity. 2nd
	 * stage length is one if conversion needs only one stage.
	 * If input and output rate is the same return 0 to
	 * use a simple copy function instead of 1 stage FIR with one
	 * tap.
	 */
	n_stages = (src->stage2->filter_length == 1) ? 1 : 2;
	if (p->idx_in == p->idx_out)
		n_stages = 0;

	/* If filter length for first stage is zero this is a deleted
	 * mode from in/out matrix. Computing of such SRC mode needs
	 * to be prevented.
	 */
	if (src->stage1->filter_length == 0)
		return -EINVAL;

	return n_stages;
}

/* Fallback function */
static void src_fallback(struct comp_dev *dev, struct comp_buffer *source,
	struct comp_buffer *sink, size_t *bytes_read, size_t *bytes_written)
{
	*bytes_read = 0;
	*bytes_written = 0;
}

/* Normal 2 stage SRC */
static void src_2s_s32_default(struct comp_dev *dev,
	struct comp_buffer *source, struct comp_buffer *sink,
	size_t *bytes_read, size_t *bytes_written)
{
	struct src_stage_prm s1;
	struct src_stage_prm s2;
	int s1_blk_in;
	int s1_blk_out;
	int s2_blk_in;
	int s2_blk_out;
	struct comp_data *cd = comp_get_drvdata(dev);
	int32_t *dest = (int32_t *)sink->w_ptr;
	int32_t *src = (int32_t *)source->r_ptr;
	int32_t *sbuf_addr = cd->delay_lines;
	int32_t *sbuf_end_addr = &cd->delay_lines[cd->param.sbuf_length];
	int32_t sbuf_size = cd->param.sbuf_length * sizeof(int32_t);
	int nch = dev->params.channels;
	int sbuf_free = cd->param.sbuf_length - cd->sbuf_avail;
	int n_read = 0;
	int n_written = 0;
	int n1 = 0;
	int n2 = 0;
	int avail_b = source->avail;
	int free_b = sink->free;
	int sz = sizeof(int32_t);

	s1.x_end_addr = source->end_addr;
	s1.x_size = source->size;
	s1.y_addr = sbuf_addr;
	s1.y_end_addr = sbuf_end_addr;
	s1.y_size = sbuf_size;
	s1.state = &cd->src.state1;
	s1.stage = cd->src.stage1;
	s1.x_rptr = src;
	s1.y_wptr = cd->sbuf_w_ptr;
	s1.nch = nch;

	s2.x_end_addr = sbuf_end_addr;
	s2.x_size = sbuf_size;
	s2.y_addr = sink->addr;
	s2.y_end_addr = sink->end_addr;
	s2.y_size = sink->size;
	s2.state = &cd->src.state2;
	s2.stage = cd->src.stage2;
	s2.x_rptr = cd->sbuf_r_ptr;
	s2.y_wptr = dest;
	s2.nch = nch;

	/* Test if 1st stage can be run with default block length to reach
	 * the period length or just under it.
	 */
	s1.times = cd->param.stage1_times;
	s1_blk_in = s1.times * cd->src.stage1->blk_in * nch;
	s1_blk_out = s1.times * cd->src.stage1->blk_out * nch;
	if (avail_b >= s1_blk_in * sz && sbuf_free >= s1_blk_out) {
		cd->polyphase_func(&s1);

		cd->sbuf_w_ptr = s1.y_wptr;
		cd->sbuf_avail += s1_blk_out;
		n_read += s1_blk_in;
		avail_b -= s1_blk_in * sz;
		sbuf_free -= s1_blk_out;
		n1 = s1.times;
	}

	/* Run one block at time the remaining data for 1st stage. */
	s1.times = 1;
	s1_blk_in = cd->src.stage1->blk_in * nch;
	s1_blk_out = cd->src.stage1->blk_out * nch;
	while (n1 < cd->param.stage1_times_max &&
		avail_b >= s1_blk_in * sz &&
		sbuf_free >= s1_blk_out) {
		cd->polyphase_func(&s1);

		cd->sbuf_w_ptr = s1.y_wptr;
		cd->sbuf_avail += s1_blk_out;
		n_read += s1_blk_in;
		avail_b -= s1_blk_in * sz;
		sbuf_free -= s1_blk_out;
		n1 += s1.times;
	}

	/* Test if 2nd stage can be run with default block length. */
	s2.times = cd->param.stage2_times;
	s2_blk_in = s2.times * cd->src.stage2->blk_in * nch;
	s2_blk_out = s2.times * cd->src.stage2->blk_out * nch;
	if (cd->sbuf_avail >= s2_blk_in && free_b >= s2_blk_out * sz) {
		cd->polyphase_func(&s2);

		cd->sbuf_r_ptr = s2.x_rptr;
		cd->sbuf_avail -= s2_blk_in;
		free_b -= s2_blk_out * sz;
		n_written += s2_blk_out;
		n2 = s2.times;
	}

	/* Run one block at time the remaining 2nd stage output */
	s2.times = 1;
	s2_blk_in = cd->src.stage2->blk_in * nch;
	s2_blk_out = cd->src.stage2->blk_out * nch;
	while (n2 < cd->param.stage2_times_max &&
		cd->sbuf_avail >= s2_blk_in &&
		free_b >= s2_blk_out * sz) {
		cd->polyphase_func(&s2);

		cd->sbuf_r_ptr = s2.x_rptr;
		cd->sbuf_avail -= s2_blk_in;
		free_b -= s2_blk_out * sz;
		n_written += s2_blk_out;
		n2 += s2.times;
	}
	*bytes_read = sizeof(int32_t) * n_read;
	*bytes_written = sizeof(int32_t) * n_written;
}

/* 1 stage SRC for simple conversions */
static void src_1s_s32_default(struct comp_dev *dev,
	struct comp_buffer *source, struct comp_buffer *sink,
	size_t *bytes_read, size_t *bytes_written)
{
	struct src_stage_prm s1;
	struct comp_data *cd = comp_get_drvdata(dev);
	int nch = dev->params.channels;
	int n_read = 0;
	int n_written = 0;

	s1.times = cd->param.stage1_times;
	s1.x_rptr = (int32_t *)source->r_ptr;
	s1.x_end_addr = source->end_addr;
	s1.x_size = source->size;
	s1.y_wptr = (int32_t *)sink->w_ptr;
	s1.y_end_addr = sink->end_addr;
	s1.y_size = sink->size;
	s1.state = &cd->src.state1;
	s1.stage = cd->src.stage1;
	s1.nch = dev->params.channels;

	cd->polyphase_func(&s1);

	n_read += nch * cd->param.blk_in;
	n_written += nch * cd->param.blk_out;
	*bytes_read = n_read * sizeof(int32_t);
	*bytes_written = n_written * sizeof(int32_t);
}

/* A fast copy function for same in and out rate */
static void src_copy_s32_default(struct comp_dev *dev,
	struct comp_buffer *source, struct comp_buffer *sink,
	size_t *bytes_read, size_t *bytes_written)
{
	struct comp_data *cd = comp_get_drvdata(dev);
	int32_t *src = (int32_t *)source->r_ptr;
	int32_t *snk = (int32_t *)sink->w_ptr;
	int nch = dev->params.channels;
	int frames = cd->param.blk_in;
	int n;
	int n_wrap_src;
	int n_wrap_snk;
	int n_wrap_min;
	int n_copy;

	n = frames * nch;
	while (n > 0) {
		n_wrap_src = (int32_t *)source->end_addr - src;
		n_wrap_snk = (int32_t *)sink->end_addr - snk;
		n_wrap_min = (n_wrap_src < n_wrap_snk) ?
			n_wrap_src : n_wrap_snk;
		n_copy = (n < n_wrap_min) ? n : n_wrap_min;
		memcpy(snk, src, n_copy * sizeof(int32_t));

		/* Update and check both source and destination for wrap */
		n -= n_copy;
		src += n_copy;
		snk += n_copy;
		src_circ_inc_wrap(&src, source->end_addr, source->size);
		src_circ_inc_wrap(&snk, sink->end_addr, sink->size);
	}
	*bytes_read = frames * nch * sizeof(int32_t);
	*bytes_written = frames * nch * sizeof(int32_t);
}

static struct comp_dev *src_new(struct sof_ipc_comp *comp)
{
	struct comp_dev *dev;
	struct sof_ipc_comp_src *src;
	struct sof_ipc_comp_src *ipc_src = (struct sof_ipc_comp_src *)comp;
	struct comp_data *cd;

	trace_src("new");

	/* validate init data - either SRC sink or source rate must be set */
	if (ipc_src->source_rate == 0 && ipc_src->sink_rate == 0) {
		trace_src_error("sn1");
		return NULL;
	}

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

	src = (struct sof_ipc_comp_src *)&dev->comp;
	memcpy(src, ipc_src, sizeof(struct sof_ipc_comp_src));

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

	comp_set_drvdata(dev, cd);

	cd->delay_lines = NULL;
	cd->src_func = src_2s_s32_default;
	cd->polyphase_func = src_polyphase_stage_cir;
	src_polyphase_reset(&cd->src);

	dev->state = COMP_STATE_READY;
	return dev;
}

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

	trace_src("fre");

	/* Free dynamically reserved buffers for SRC algorithm */
	if (!cd->delay_lines)
		rfree(cd->delay_lines);

	rfree(cd);
	rfree(dev);
}

/* set component audio stream parameters */
static int src_params(struct comp_dev *dev)
{
	struct sof_ipc_stream_params *params = &dev->params;
	struct sof_ipc_comp_src *src = COMP_GET_IPC(dev, sof_ipc_comp_src);
	struct sof_ipc_comp_config *config = COMP_GET_CONFIG(dev);
	struct comp_data *cd = comp_get_drvdata(dev);
	struct comp_buffer *sink;
	struct comp_buffer *source;
	size_t delay_lines_size;
	uint32_t source_rate;
	uint32_t sink_rate;
	int32_t *buffer_start;
	int n = 0;
	int err;
	int frames_is_for_source;
	int q;

	trace_src("par");

	/* SRC supports S24_4LE and S32_LE formats */
	switch (config->frame_fmt) {
	case SOF_IPC_FRAME_S24_4LE:
		cd->polyphase_func = src_polyphase_stage_cir_s24;
		break;
	case SOF_IPC_FRAME_S32_LE:
		cd->polyphase_func = src_polyphase_stage_cir;
		break;
	default:
		trace_src_error("sr0");
		return -EINVAL;
	}

	/* Calculate source and sink rates, one rate will come from IPC new
	 * and the other from params.
	 */
	if (src->source_rate == 0) {
		/* params rate is source rate */
		source_rate = params->rate;
		sink_rate = src->sink_rate;
		/* re-write our params with output rate for next component */
		params->rate = sink_rate;
		frames_is_for_source = 0;
	} else {
		/* params rate is sink rate */
		source_rate = src->source_rate;
		sink_rate = params->rate;
		/* re-write our params with output rate for next component */
		params->rate = source_rate;
		frames_is_for_source = 1;
	}

	/* Allocate needed memory for delay lines */
	err = src_buffer_lengths(&cd->param, source_rate, sink_rate,
		params->channels, dev->frames, frames_is_for_source);
	if (err < 0) {
		trace_src_error("sr1");
		trace_error_value(source_rate);
		trace_error_value(sink_rate);
		trace_error_value(params->channels);
		trace_error_value(dev->frames);
		return err;
	}

	delay_lines_size = sizeof(int32_t) * cd->param.total;
	if (delay_lines_size == 0) {
		trace_src_error("sr2");
		return -EINVAL;
	}

	/* free any existing delay lines. TODO reuse if same size */
	if (!cd->delay_lines)
		rfree(cd->delay_lines);

	cd->delay_lines = rballoc(RZONE_RUNTIME, SOF_MEM_CAPS_RAM,
		delay_lines_size);
	if (!cd->delay_lines) {
		trace_src_error("sr3");
		trace_error_value(delay_lines_size);
		return -EINVAL;
	}

	/* Clear all delay lines here */
	memset(cd->delay_lines, 0, delay_lines_size);
	buffer_start = cd->delay_lines + cd->param.sbuf_length;

	/* Initialize SRC for actual sample rate */
	n = src_polyphase_init(&cd->src, &cd->param, buffer_start);

	/* Reset stage buffer */
	cd->sbuf_r_ptr = cd->delay_lines;
	cd->sbuf_w_ptr = cd->delay_lines;
	cd->sbuf_avail = 0;

	switch (n) {
	case 0:
		cd->src_func = src_copy_s32_default; /* 1:1 fast copy */
		break;
	case 1:
		cd->src_func = src_1s_s32_default; /* Simpler 1 stage SRC */
		break;
	case 2:
		cd->src_func = src_2s_s32_default; /* Default 2 stage SRC */
		break;
	default:
		/* This is possibly due to missing coefficients for
		 * requested rates combination. Sink audio will be
		 * muted if copy() is run.
		 */
		trace_src("SFa");
		cd->src_func = src_fallback;
		return -EINVAL;
	}

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

	/* The downstream buffer must be at least length of blk_out plus
	 * a dev->frames and an integer multiple of dev->frames. The
	 * buffer_set_size will return an error if the required length would
	 * be too long.
	 */
	q = src_ceil_divide(cd->param.blk_out, (int)dev->frames) + 1;

	/* Configure downstream buffer */
	sink = list_first_item(&dev->bsink_list, struct comp_buffer,
		source_list);
	err = buffer_set_size(sink, q * dev->frames * dev->frame_bytes);
	if (err < 0) {
		trace_src_error("eSz");
		trace_error_value(sink->alloc_size);
		trace_error_value(q * dev->frames * dev->frame_bytes);
		return err;
	}

	/* Check that source buffer has sufficient size */
	source = list_first_item(&dev->bsource_list, struct comp_buffer,
		sink_list);
	if (source->size < cd->param.blk_in * dev->frame_bytes) {
		trace_src_error("eSy");
		return -EINVAL;
	}

	return 0;
}

static int src_ctrl_cmd(struct comp_dev *dev, struct sof_ipc_ctrl_data *cdata)
{
	trace_src_error("ec1");
	return -EINVAL;
}

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

	trace_src("cmd");

	if (cmd == COMP_CMD_SET_VALUE)
		ret = src_ctrl_cmd(dev, cdata);

	return ret;
}

static int src_trigger(struct comp_dev *dev, int cmd)
{
	trace_src("trg");

	return comp_set_state(dev, cmd);
}

/* copy and process stream data from source to sink buffers */
static int src_copy(struct comp_dev *dev)
{
	struct comp_data *cd = comp_get_drvdata(dev);
	struct comp_buffer *source;
	struct comp_buffer *sink;
	int need_source;
	int need_sink;
	size_t consumed = 0;
	size_t produced = 0;

	trace_src("SRC");

	/* src component needs 1 source and 1 sink buffer */
	source = list_first_item(&dev->bsource_list, struct comp_buffer,
		sink_list);
	sink = list_first_item(&dev->bsink_list, struct comp_buffer,
		source_list);

	/* Calculate needed amount of source buffer and sink buffer
	 * for one SRC run. The blk_in and blk are minimum condition to
	 * call copy. Copy can consume or produce a slightly larger block
	 * with the rates where block sizes are not constant. E.g. for
	 * 1 ms scheduling the blocks can be under or above 1 ms when the
	 * SRC interval block size constraint prevents exact 1 ms blocks.
	 */
	need_source = cd->param.blk_in * dev->frame_bytes;
	need_sink = cd->param.blk_out * dev->frame_bytes;

	/* 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.
	 */
	if (source->avail < need_source) {
		trace_src_error("xru");
		return -EIO;	/* xrun */
	}
	if (sink->free < need_sink) {
		trace_src_error("xro");
		return -EIO;	/* xrun */
	}

	cd->src_func(dev, source, sink, &consumed, &produced);

	tracev_value(consumed >> 3);
	tracev_value(produced >> 3);

	/* Calc new free and available if data was processed. These
	 * functions must not be called with 0 consumed/produced.
	 */
	if (consumed > 0)
		comp_update_buffer_consume(source, consumed);

	if (produced > 0) {
		comp_update_buffer_produce(sink, produced);
		return cd->param.blk_out;
	}

	/* produced no data */
	return 0;
}

static int src_prepare(struct comp_dev *dev)
{
	trace_src("pre");

	return comp_set_state(dev, COMP_TRIGGER_PREPARE);
}

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

	trace_src("SRe");

	cd->src_func = src_2s_s32_default;
	src_polyphase_reset(&cd->src);

	comp_set_state(dev, COMP_TRIGGER_RESET);
	return 0;
}

struct comp_driver comp_src = {
	.type = SOF_COMP_SRC,
	.ops = {
		.new = src_new,
		.free = src_free,
		.params = src_params,
		.cmd = src_cmd,
		.trigger = src_trigger,
		.copy = src_copy,
		.prepare = src_prepare,
		.reset = src_reset,
	},
};

void sys_comp_src_init(void)
{
	comp_register(&comp_src);
}