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 <reef/reef.h>
#include <reef/lock.h>
#include <reef/list.h>
#include <reef/stream.h>
#include <reef/alloc.h>
#include <reef/work.h>
#include <reef/clock.h>
#include <reef/audio/component.h>
#include <reef/audio/pipeline.h>
#include <uapi/ipc.h>
#include "src_core.h"

#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)

/* src component private data */
struct comp_data {
	struct polyphase_src src[PLATFORM_MAX_CHANNELS];
	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;
	int sign_extend_s24; /* Set if need to copy sign bit to b24..b31 */
	void (* src_func)(struct comp_dev *dev,
		struct comp_buffer *source,
		struct comp_buffer *sink,
		size_t *consumed,
		size_t *produced);
};

/* Fallback function */
static void fallback_s32(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 polyphase_src *s;
	struct src_stage_prm s1;
	struct src_stage_prm s2;
	int j;
	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);
	const int nch = dev->params.channels;
	const int s1_blk_in = cd->src[0].stage1->blk_in * nch;
	const int s1_blk_out = cd->src[0].stage1->blk_out * nch;
	const int s2_blk_in = cd->src[0].stage2->blk_in * nch;
	const int s2_blk_out = cd->src[0].stage2->blk_out * nch;
	int sbuf_free = cd->param.sbuf_length - cd->sbuf_avail;
	int source_check = (source->avail >> 2) - s1_blk_in; /* for int32_t */
	int sink_check = (sink->free >> 2) - s2_blk_out; /* For int32_t */
	int n_read = 0;
	int n_written = 0;
	int n1 = 0;
	int n2 = 0;

	s1.times = 1;
	s1.x_end_addr = source->end_addr;
	s1.x_size = source->size;
	s1.x_inc = nch;
	s1.y_end_addr = sbuf_end_addr;
	s1.y_size = sbuf_size;
	s1.y_inc = nch;

	s2.times = 1;
	s2.x_end_addr = sbuf_end_addr;
	s2.x_size = sbuf_size;
	s2.x_inc = nch;
	s2.y_end_addr = sink->end_addr;
	s2.y_size = sink->size;
	s2.y_inc = nch;

	/* 1st stage runs once a long multiplied length block.
	 * The stage buffer much be large enough to fit one s1 output block
	 * plus one s2 input block plus jitter in s2 consumption.
	 */
	while ((n1 < cd->param.stage1_times_max)
		&& (n_read <= source_check)
		&& (sbuf_free >= s1_blk_out)) {
		for (j = 0; j < nch; j++) {
			s = &cd->src[j]; /* Point to src[] for this channel */
			s1.x_rptr = src++;
			s1.y_wptr = cd->sbuf_w_ptr++;
			src_circ_inc_wrap(&src, source->end_addr, source->size);
			src_circ_inc_wrap(&cd->sbuf_w_ptr, sbuf_end_addr, sbuf_size);
			s1.state = &s->state1;
			s1.stage = s->stage1;
			if (cd->sign_extend_s24)
				src_polyphase_stage_cir_s24(&s1);
			else
				src_polyphase_stage_cir(&s1);

		}
		n_read += s1_blk_in;
		cd->sbuf_avail += s1_blk_out;
		sbuf_free -= s1_blk_out;
		src = s1.x_rptr - nch + 1;
		cd->sbuf_w_ptr = s1.y_wptr - nch + 1;
		src_circ_dec_wrap(&src, source->addr, source->size);
		src_circ_dec_wrap(&cd->sbuf_w_ptr, sbuf_addr, sbuf_size);
		n1++;
	}

	/* 2nd stage runs as many min size blocks as buffers allow */
	while ((n2 < cd->param.stage2_times_max)
		&& (cd->sbuf_avail >= s2_blk_in)
		&& (n_written <= sink_check)) {
		for (j = 0; j < nch; j++) {
			s2.x_rptr = cd->sbuf_r_ptr++;
			s2.y_wptr = dest++;
			src_circ_inc_wrap(&cd->sbuf_r_ptr, sbuf_end_addr, sbuf_size);
			src_circ_inc_wrap(&dest, sink->end_addr, sink->size);
			s = &cd->src[j]; /* Point to src[] for this channel */
			s2.state = &s->state2;
			s2.stage = s->stage2;
			if (cd->sign_extend_s24)
				src_polyphase_stage_cir_s24(&s2);
			else
				src_polyphase_stage_cir(&s2);

		}
		cd->sbuf_r_ptr = s2.x_rptr - nch + 1;
		dest = s2.y_wptr - nch + 1;
		src_circ_dec_wrap(&cd->sbuf_r_ptr, sbuf_addr, sbuf_size);
		src_circ_dec_wrap(&dest, sink->addr, sink->size);
		n_written += s2_blk_out;
		cd->sbuf_avail -= s2_blk_in;
		n2++;
	}
	*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 polyphase_src *s;
	struct src_stage_prm s1;
	int j;
	struct comp_data *cd = comp_get_drvdata(dev);
	int32_t *dest = (int32_t *) sink->w_ptr;
	int32_t *src = (int32_t *) source->r_ptr;
	int nch = dev->params.channels;
	int n_read = 0;
	int n_written = 0;

	s1.times = cd->param.stage1_times;
	s1.x_end_addr = source->end_addr;
	s1.x_size = source->size;
	s1.x_inc = nch;
	s1.y_end_addr = sink->end_addr;
	s1.y_size = sink->size;
	s1.y_inc = nch;
	s1.x_rptr = src + nch - 1;
	s1.y_wptr = dest + nch - 1;

	for (j = 0; j < nch; j++) {
		s = &cd->src[j]; /* Point to src for this channel */
		s1.x_rptr = src++;
		s1.y_wptr = dest++;
		src_circ_inc_wrap(&src, source->end_addr, source->size);
		src_circ_inc_wrap(&dest, sink->end_addr, sink->size);
		s1.state = &s->state1;
		s1.stage = s->stage1;
		if (cd->sign_extend_s24)
			src_polyphase_stage_cir_s24(&s1);
		else
			src_polyphase_stage_cir(&s1);

		n_read += cd->param.blk_in;
		n_written += 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;
	int i;

	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, RFLAGS_NONE,
		COMP_SIZE(struct sof_ipc_comp_src));
	if (dev == NULL)
		return NULL;

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

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

	comp_set_drvdata(dev, cd);

	cd->delay_lines = NULL;
	cd->src_func = src_2s_s32_default;
	for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)
		src_polyphase_reset(&cd->src[i]);

	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 != NULL)
		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 i;
	int err;
	int frames_is_for_source;
	int nch;
	int q;

	trace_src("par");

	/* SRC supports S24_4LE and S32_LE formats */
	switch (config->frame_fmt) {
	case SOF_IPC_FRAME_S24_4LE:
		cd->sign_extend_s24 = 1;
		break;
	case SOF_IPC_FRAME_S32_LE:
		cd->sign_extend_s24 = 0;
		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_value(source_rate);
		trace_value(sink_rate);
		trace_value(params->channels);
		trace_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 dalay lines. TODO reuse if same size */
	if (cd->delay_lines != NULL)
		rfree(cd->delay_lines);

	cd->delay_lines = rballoc(RZONE_RUNTIME, RFLAGS_NONE, delay_lines_size);
	if (cd->delay_lines == NULL) {
		trace_src_error("sr3");
		trace_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;

	/* Initize SRC for actual sample rate */
	nch = MIN(params->channels, PLATFORM_MAX_CHANNELS);
	for (i = 0; i < nch; i++) {
		n = src_polyphase_init(&cd->src[i], &cd->param, buffer_start);
		buffer_start += cd->param.single_src;
	}

	/* 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 = fallback_s32;
		return -EINVAL;
		break;
	}

	/* 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_value(sink->alloc_size);
		trace_value(q * dev->frames * dev->frame_bytes);
		return err;
	}

	buffer_reset_pos(sink);

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

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

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

	return ret;
}

/* 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_comp("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 schduling the blocks can be under or above 1 ms when the
	 * SRC interal 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;

	/* Run SRC function if buffers avail and free allow */
	if (((int) source->avail >= need_source) && ((int) sink->free >= need_sink)) {
		/* Run src */
		cd->src_func(dev, source, sink, &consumed, &produced);

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

static int src_prepare(struct comp_dev *dev)
{
	dev->state = COMP_STATE_PREPARE;
	return 0;
}

static int src_preload(struct comp_dev *dev)
{
	return src_copy(dev);
}

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

	trace_src("SRe");

	cd->src_func = src_2s_s32_default;
	for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)
		src_polyphase_reset(&cd->src[i]);

	dev->state = COMP_STATE_READY;
	return 0;
}

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

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