blob: d494d966fe5c2d4d228630bab15f4a7499eeb8c8 [file] [log] [blame]
Seppo Ingalsuo6a274832017-06-07 14:17:55 +03001/*
2 * Copyright (c) 2017, Intel Corporation
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the Intel Corporation nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 * POSSIBILITY OF SUCH DAMAGE.
27 *
28 * Author: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>
29 * Liam Girdwood <liam.r.girdwood@linux.intel.com>
30 * Keyon Jie <yang.jie@linux.intel.com>
31 */
32
33#include <stdint.h>
34#include <stddef.h>
35#include <errno.h>
36#include <reef/reef.h>
37#include <reef/lock.h>
38#include <reef/list.h>
39#include <reef/stream.h>
40#include <reef/alloc.h>
41#include <reef/work.h>
42#include <reef/clock.h>
43#include <reef/audio/component.h>
44#include <reef/audio/pipeline.h>
Liam Girdwood17641522017-06-09 17:27:02 +010045#include <uapi/ipc.h>
Seppo Ingalsuo6a274832017-06-07 14:17:55 +030046#include "src_core.h"
47
48#ifdef MODULE_TEST
49#include <stdio.h>
50#endif
51
52#define trace_src(__e) trace_event(TRACE_CLASS_SRC, __e)
53#define tracev_src(__e) tracev_event(TRACE_CLASS_SRC, __e)
54#define trace_src_error(__e) trace_error(TRACE_CLASS_SRC, __e)
55
56/* src component private data */
57struct comp_data {
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +030058 struct polyphase_src src;
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +030059 struct src_param param;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +030060 int32_t *delay_lines;
Liam Girdwood3488cce2017-08-10 11:59:08 +010061 uint32_t sink_rate;
62 uint32_t source_rate;
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +030063 int32_t *sbuf_w_ptr;
64 int32_t *sbuf_r_ptr;
65 int sbuf_avail;
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +030066 void (* src_func)(struct comp_dev *dev,
Seppo Ingalsuo6a274832017-06-07 14:17:55 +030067 struct comp_buffer *source,
68 struct comp_buffer *sink,
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +030069 size_t *consumed,
70 size_t *produced);
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +030071 void (* polyphase_func)(struct src_stage_prm *s);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +030072};
73
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +030074/* Fallback function */
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +030075static void src_fallback(struct comp_dev *dev, struct comp_buffer *source,
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +030076 struct comp_buffer *sink, size_t *bytes_read, size_t *bytes_written)
Seppo Ingalsuo6a274832017-06-07 14:17:55 +030077{
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +030078 *bytes_read = 0;
79 *bytes_written = 0;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +030080}
81
82/* Normal 2 stage SRC */
83static void src_2s_s32_default(struct comp_dev *dev,
84 struct comp_buffer *source, struct comp_buffer *sink,
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +030085 size_t *bytes_read, size_t *bytes_written)
Seppo Ingalsuo6a274832017-06-07 14:17:55 +030086{
Pierre-Louis Bossart4ccf81d2017-09-25 14:52:09 -050087 struct src_stage_prm s1;
88 struct src_stage_prm s2;
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +030089 int s1_blk_in;
90 int s1_blk_out;
91 int s2_blk_in;
92 int s2_blk_out;
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +030093 struct comp_data *cd = comp_get_drvdata(dev);
94 int32_t *dest = (int32_t *) sink->w_ptr;
95 int32_t *src = (int32_t *) source->r_ptr;
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +030096 int32_t *sbuf_end_addr = &cd->delay_lines[cd->param.sbuf_length];
97 int32_t sbuf_size = cd->param.sbuf_length * sizeof(int32_t);
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +030098 int nch = dev->params.channels;
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +030099 int sbuf_free = cd->param.sbuf_length - cd->sbuf_avail;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300100 int n_read = 0;
101 int n_written = 0;
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300102 int n1 = 0;
103 int n2 = 0;
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300104 int avail_b = source->avail;
105 int free_b = sink->free;
106 int sz = sizeof(int32_t);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300107
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300108 s1.x_end_addr = source->end_addr;
Liam Girdwood44509242017-09-07 15:34:42 +0100109 s1.x_size = source->size;
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300110 s1.y_end_addr = sbuf_end_addr;
111 s1.y_size = sbuf_size;
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300112 s1.state = &cd->src.state1;
113 s1.stage = cd->src.stage1;
114 s1.x_rptr = src;
115 s1.y_wptr = cd->sbuf_w_ptr;
116 s1.nch = nch;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300117
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300118 s2.x_end_addr = sbuf_end_addr;
119 s2.x_size = sbuf_size;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300120 s2.y_end_addr = sink->end_addr;
Liam Girdwood44509242017-09-07 15:34:42 +0100121 s2.y_size = sink->size;
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300122 s2.state = &cd->src.state2;
123 s2.stage = cd->src.stage2;
124 s2.x_rptr = cd->sbuf_r_ptr;
125 s2.y_wptr = dest;
126 s2.nch = nch;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300127
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300128
129 /* Test if 1st stage can be run with default block length to reach
130 * the period length or just under it.
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300131 */
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300132 s1.times = cd->param.stage1_times;
133 s1_blk_in = s1.times * cd->src.stage1->blk_in * nch;
134 s1_blk_out = s1.times * cd->src.stage1->blk_out * nch;
135 if ((avail_b >= s1_blk_in * sz) && (sbuf_free >= s1_blk_out)) {
136 cd->polyphase_func(&s1);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300137
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300138 cd->sbuf_w_ptr = s1.y_wptr;
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300139 cd->sbuf_avail += s1_blk_out;
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300140 n_read += s1_blk_in;
141 avail_b -= s1_blk_in * sz;
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300142 sbuf_free -= s1_blk_out;
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300143 n1 = s1.times;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300144 }
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300145
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300146 /* Run one block at time the remaining data for 1st stage. */
147 s1.times = 1;
148 s1_blk_in = cd->src.stage1->blk_in * nch;
149 s1_blk_out = cd->src.stage1->blk_out * nch;
150 while ((n1 < cd->param.stage1_times_max) && (avail_b >= s1_blk_in * sz)
151 && (sbuf_free >= s1_blk_out)) {
152 cd->polyphase_func(&s1);
153
154 cd->sbuf_w_ptr = s1.y_wptr;
155 cd->sbuf_avail += s1_blk_out;
156 n_read += s1_blk_in;
157 avail_b -= s1_blk_in * sz;
158 sbuf_free -= s1_blk_out;
159 n1 += s1.times;
160 }
161
162 /* Test if 2nd stage can be run with default block length. */
163 s2.times = cd->param.stage2_times;
164 s2_blk_in = s2.times * cd->src.stage2->blk_in * nch;
165 s2_blk_out = s2.times * cd->src.stage2->blk_out * nch;
166 if ((cd->sbuf_avail >= s2_blk_in) && (free_b >= s2_blk_out * sz)) {
167 cd->polyphase_func(&s2);
168
169 cd->sbuf_r_ptr = s2.x_rptr;
170 cd->sbuf_avail -= s2_blk_in;
171 free_b -= s2_blk_out * sz;
172 n_written += s2_blk_out;
173 n2 = s2.times;
174 }
175
176
177 /* Run one block at time the remaining 2nd stage output */
178 s2.times = 1;
179 s2_blk_in = cd->src.stage2->blk_in * nch;
180 s2_blk_out = cd->src.stage2->blk_out * nch;
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300181 while ((n2 < cd->param.stage2_times_max)
182 && (cd->sbuf_avail >= s2_blk_in)
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300183 && (free_b >= s2_blk_out * sz)) {
184 cd->polyphase_func(&s2);
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300185
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300186 cd->sbuf_r_ptr = s2.x_rptr;
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300187 cd->sbuf_avail -= s2_blk_in;
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300188 free_b -= s2_blk_out * sz;
189 n_written += s2_blk_out;
190 n2 += s2.times;
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300191 }
192 *bytes_read = sizeof(int32_t) * n_read;
193 *bytes_written = sizeof(int32_t) * n_written;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300194}
195
196/* 1 stage SRC for simple conversions */
197static void src_1s_s32_default(struct comp_dev *dev,
198 struct comp_buffer *source, struct comp_buffer *sink,
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300199 size_t *bytes_read, size_t *bytes_written)
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300200{
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300201 struct src_stage_prm s1;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300202 struct comp_data *cd = comp_get_drvdata(dev);
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300203 int nch = dev->params.channels;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300204 int n_read = 0;
205 int n_written = 0;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300206
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300207 s1.times = cd->param.stage1_times;
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300208 s1.x_rptr = (int32_t *) source->r_ptr;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300209 s1.x_end_addr = source->end_addr;
Liam Girdwood44509242017-09-07 15:34:42 +0100210 s1.x_size = source->size;
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300211 s1.y_wptr = (int32_t *) sink->w_ptr;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300212 s1.y_end_addr = sink->end_addr;
Liam Girdwood44509242017-09-07 15:34:42 +0100213 s1.y_size = sink->size;
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300214 s1.state = &cd->src.state1;
215 s1.stage = cd->src.stage1;
216 s1.nch = dev->params.channels;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300217
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300218 cd->polyphase_func(&s1);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300219
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300220 n_read += nch * cd->param.blk_in;
221 n_written += nch * cd->param.blk_out;
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300222 *bytes_read = n_read * sizeof(int32_t);
223 *bytes_written = n_written * sizeof(int32_t);
224}
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300225
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300226/* A fast copy function for same in and out rate */
227static void src_copy_s32_default(struct comp_dev *dev,
228 struct comp_buffer *source, struct comp_buffer *sink,
229 size_t *bytes_read, size_t *bytes_written)
230{
231 struct comp_data *cd = comp_get_drvdata(dev);
232 int32_t *src = (int32_t *) source->r_ptr;
233 int32_t *snk = (int32_t *) sink->w_ptr;
234 int nch = dev->params.channels;
235 int frames = cd->param.blk_in;
236 int n;
237 int n_wrap_src;
238 int n_wrap_snk;
239 int n_wrap_min;
240 int n_copy;
241
242 n = frames * nch;
243 while (n > 0) {
244 n_wrap_src = (int32_t *) source->end_addr - src;
245 n_wrap_snk = (int32_t *) sink->end_addr - snk;
246 n_wrap_min = (n_wrap_src < n_wrap_snk) ? n_wrap_src : n_wrap_snk;
247 n_copy = (n < n_wrap_min) ? n : n_wrap_min;
248 memcpy(snk, src, n_copy * sizeof(int32_t));
249
250 /* Update and check both source and destination for wrap */
251 n -= n_copy;
252 src += n_copy;
253 snk += n_copy;
254 src_circ_inc_wrap(&src, source->end_addr, source->size);
255 src_circ_inc_wrap(&snk, sink->end_addr, sink->size);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300256
257 }
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300258 *bytes_read = frames * nch * sizeof(int32_t);
259 *bytes_written = frames * nch * sizeof(int32_t);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300260}
261
262static struct comp_dev *src_new(struct sof_ipc_comp *comp)
263{
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300264 struct comp_dev *dev;
Liam Girdwood960cf8e2017-06-12 11:38:07 +0100265 struct sof_ipc_comp_src *src;
Seppo Ingalsuo28ef8542017-08-25 18:29:04 +0300266 struct sof_ipc_comp_src *ipc_src = (struct sof_ipc_comp_src *) comp;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300267 struct comp_data *cd;
268
Liam Girdwood3488cce2017-08-10 11:59:08 +0100269 trace_src("new");
Liam Girdwood960cf8e2017-06-12 11:38:07 +0100270
Liam Girdwoodb12aa0d2017-09-03 22:00:58 +0100271 /* validate init data - either SRC sink or source rate must be set */
272 if (ipc_src->source_rate == 0 && ipc_src->sink_rate == 0) {
273 trace_src_error("sn1");
274 return NULL;
275 }
276
Liam Girdwood3488cce2017-08-10 11:59:08 +0100277 dev = rzalloc(RZONE_RUNTIME, RFLAGS_NONE,
Liam Girdwood17641522017-06-09 17:27:02 +0100278 COMP_SIZE(struct sof_ipc_comp_src));
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300279 if (dev == NULL)
280 return NULL;
281
Seppo Ingalsuo28ef8542017-08-25 18:29:04 +0300282 src = (struct sof_ipc_comp_src *) &dev->comp;
Liam Girdwood960cf8e2017-06-12 11:38:07 +0100283 memcpy(src, ipc_src, sizeof(struct sof_ipc_comp_src));
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300284
Liam Girdwood3488cce2017-08-10 11:59:08 +0100285 cd = rzalloc(RZONE_RUNTIME, RFLAGS_NONE, sizeof(*cd));
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300286 if (cd == NULL) {
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300287 rfree(dev);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300288 return NULL;
289 }
290
291 comp_set_drvdata(dev, cd);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300292
293 cd->delay_lines = NULL;
294 cd->src_func = src_2s_s32_default;
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300295 cd->polyphase_func = src_polyphase_stage_cir;
296 src_polyphase_reset(&cd->src);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300297
Liam Girdwoodbe41b682017-09-21 16:48:18 +0100298 dev->state = COMP_STATE_READY;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300299 return dev;
300}
301
302static void src_free(struct comp_dev *dev)
303{
304 struct comp_data *cd = comp_get_drvdata(dev);
305
Liam Girdwood3488cce2017-08-10 11:59:08 +0100306 trace_src("fre");
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300307
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300308 /* Free dynamically reserved buffers for SRC algorithm */
309 if (cd->delay_lines != NULL)
310 rfree(cd->delay_lines);
311
312 rfree(cd);
313 rfree(dev);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300314}
315
316/* set component audio stream parameters */
Liam Girdwood2cfaebe2017-08-21 17:13:52 +0100317static int src_params(struct comp_dev *dev)
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300318{
Liam Girdwood3488cce2017-08-10 11:59:08 +0100319 struct sof_ipc_stream_params *params = &dev->params;
320 struct sof_ipc_comp_src *src = COMP_GET_IPC(dev, sof_ipc_comp_src);
321 struct sof_ipc_comp_config *config = COMP_GET_CONFIG(dev);
322 struct comp_data *cd = comp_get_drvdata(dev);
Pierre-Louis Bossart4ccf81d2017-09-25 14:52:09 -0500323 struct comp_buffer *sink;
324 struct comp_buffer *source;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300325 size_t delay_lines_size;
Pierre-Louis Bossart4ccf81d2017-09-25 14:52:09 -0500326 uint32_t source_rate;
327 uint32_t sink_rate;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300328 int32_t *buffer_start;
Pierre-Louis Bossart4ccf81d2017-09-25 14:52:09 -0500329 int n = 0;
Pierre-Louis Bossart4ccf81d2017-09-25 14:52:09 -0500330 int err;
331 int frames_is_for_source;
Pierre-Louis Bossart4ccf81d2017-09-25 14:52:09 -0500332 int q;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300333
Liam Girdwood3488cce2017-08-10 11:59:08 +0100334 trace_src("par");
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300335
Seppo Ingalsuo887628a2017-09-14 16:00:05 +0300336 /* SRC supports S24_4LE and S32_LE formats */
337 switch (config->frame_fmt) {
338 case SOF_IPC_FRAME_S24_4LE:
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300339 cd->polyphase_func = src_polyphase_stage_cir_s24;
Seppo Ingalsuo887628a2017-09-14 16:00:05 +0300340 break;
341 case SOF_IPC_FRAME_S32_LE:
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300342 cd->polyphase_func = src_polyphase_stage_cir;
Seppo Ingalsuo887628a2017-09-14 16:00:05 +0300343 break;
344 default:
Liam Girdwoodb12aa0d2017-09-03 22:00:58 +0100345 trace_src_error("sr0");
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300346 return -EINVAL;
Liam Girdwoodb12aa0d2017-09-03 22:00:58 +0100347 }
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300348
Liam Girdwood3488cce2017-08-10 11:59:08 +0100349 /* Calculate source and sink rates, one rate will come from IPC new
350 * and the other from params. */
351 if (src->source_rate == 0) {
352 /* params rate is source rate */
353 source_rate = params->rate;
354 sink_rate = src->sink_rate;
355 /* re-write our params with output rate for next component */
356 params->rate = sink_rate;
Seppo Ingalsuocb3a64d2017-09-06 15:16:12 +0300357 frames_is_for_source = 0;
Liam Girdwood3488cce2017-08-10 11:59:08 +0100358 } else {
359 /* params rate is sink rate */
360 source_rate = src->source_rate;
361 sink_rate = params->rate;
362 /* re-write our params with output rate for next component */
363 params->rate = source_rate;
Seppo Ingalsuocb3a64d2017-09-06 15:16:12 +0300364 frames_is_for_source = 1;
Liam Girdwood3488cce2017-08-10 11:59:08 +0100365 }
Seppo Ingalsuo4a006912017-06-28 19:25:51 +0300366
367 /* Allocate needed memory for delay lines */
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300368 err = src_buffer_lengths(&cd->param, source_rate, sink_rate,
Seppo Ingalsuocb3a64d2017-09-06 15:16:12 +0300369 params->channels, dev->frames, frames_is_for_source);
Liam Girdwoodb12aa0d2017-09-03 22:00:58 +0100370 if (err < 0) {
371 trace_src_error("sr1");
372 trace_value(source_rate);
373 trace_value(sink_rate);
374 trace_value(params->channels);
Seppo Ingalsuocb3a64d2017-09-06 15:16:12 +0300375 trace_value(dev->frames);
Liam Girdwoodb12aa0d2017-09-03 22:00:58 +0100376 return err;
377 }
378
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300379 delay_lines_size = sizeof(int32_t) * cd->param.total;
Liam Girdwoodb12aa0d2017-09-03 22:00:58 +0100380 if (delay_lines_size == 0) {
381 trace_src_error("sr2");
382 return -EINVAL;
383 }
384
Pierre-Louis Bossartf9458092017-11-09 15:24:07 -0600385 /* free any existing delay lines. TODO reuse if same size */
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300386 if (cd->delay_lines != NULL)
387 rfree(cd->delay_lines);
388
Liam Girdwood22ce8062017-09-03 22:04:26 +0100389 cd->delay_lines = rballoc(RZONE_RUNTIME, RFLAGS_NONE, delay_lines_size);
Liam Girdwoodb12aa0d2017-09-03 22:00:58 +0100390 if (cd->delay_lines == NULL) {
391 trace_src_error("sr3");
392 trace_value(delay_lines_size);
Liam Girdwood3488cce2017-08-10 11:59:08 +0100393 return -EINVAL;
Liam Girdwoodb12aa0d2017-09-03 22:00:58 +0100394 }
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300395
396 /* Clear all delay lines here */
397 memset(cd->delay_lines, 0, delay_lines_size);
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300398 buffer_start = cd->delay_lines + cd->param.sbuf_length;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300399
Pierre-Louis Bossartf9458092017-11-09 15:24:07 -0600400 /* Initialize SRC for actual sample rate */
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300401 n = src_polyphase_init(&cd->src, &cd->param, buffer_start);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300402
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300403 /* Reset stage buffer */
404 cd->sbuf_r_ptr = cd->delay_lines;
405 cd->sbuf_w_ptr = cd->delay_lines;
406 cd->sbuf_avail = 0;
407
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300408 switch (n) {
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300409 case 0:
410 cd->src_func = src_copy_s32_default; /* 1:1 fast copy */
411 break;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300412 case 1:
413 cd->src_func = src_1s_s32_default; /* Simpler 1 stage SRC */
414 break;
415 case 2:
416 cd->src_func = src_2s_s32_default; /* Default 2 stage SRC */
417 break;
418 default:
419 /* This is possibly due to missing coefficients for
420 * requested rates combination. Sink audio will be
421 * muted if copy() is run.
422 */
423 trace_src("SFa");
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300424 cd->src_func = src_fallback;
Seppo Ingalsuo4a006912017-06-28 19:25:51 +0300425 return -EINVAL;
Liam Girdwood3488cce2017-08-10 11:59:08 +0100426 break;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300427 }
428
Seppo Ingalsuo14d10a82017-09-22 16:08:47 +0300429 /* Calculate period size based on config. First make sure that
430 * frame_bytes is set.
431 */
Seppo Ingalsuo28ef8542017-08-25 18:29:04 +0300432 dev->frame_bytes =
433 dev->params.sample_container_bytes * dev->params.channels;
Liam Girdwood44509242017-09-07 15:34:42 +0100434
Seppo Ingalsuo54162292017-09-12 13:45:50 +0300435 /* The downstream buffer must be at least length of blk_out plus
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300436 * a dev->frames and an integer multiple of dev->frames. The
Seppo Ingalsuo54162292017-09-12 13:45:50 +0300437 * buffer_set_size will return an error if the required length would
438 * be too long.
439 */
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300440 q = src_ceil_divide(cd->param.blk_out, (int) dev->frames) + 1;
Seppo Ingalsuo54162292017-09-12 13:45:50 +0300441
442 /* Configure downstream buffer */
443 sink = list_first_item(&dev->bsink_list, struct comp_buffer,
444 source_list);
445 err = buffer_set_size(sink, q * dev->frames * dev->frame_bytes);
Liam Girdwood44509242017-09-07 15:34:42 +0100446 if (err < 0) {
447 trace_src_error("eSz");
Seppo Ingalsuo887628a2017-09-14 16:00:05 +0300448 trace_value(sink->alloc_size);
449 trace_value(q * dev->frames * dev->frame_bytes);
Liam Girdwood44509242017-09-07 15:34:42 +0100450 return err;
451 }
452
Seppo Ingalsuo54162292017-09-12 13:45:50 +0300453 /* Check that source buffer has sufficient size */
454 source = list_first_item(&dev->bsource_list, struct comp_buffer,
455 sink_list);
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300456 if (source->size < cd->param.blk_in * dev->frame_bytes) {
Seppo Ingalsuo54162292017-09-12 13:45:50 +0300457 trace_src_error("eSy");
458 return -EINVAL;
459 }
460
461
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300462 return 0;
463}
464
Liam Girdwood854b2e52017-09-02 23:14:02 +0100465static int src_ctrl_cmd(struct comp_dev *dev, struct sof_ipc_ctrl_data *cdata)
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300466{
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300467 trace_src_error("ec1");
468 return -EINVAL;
Liam Girdwood854b2e52017-09-02 23:14:02 +0100469}
470
471/* used to pass standard and bespoke commands (with data) to component */
472static int src_cmd(struct comp_dev *dev, int cmd, void *data)
473{
474 struct sof_ipc_ctrl_data *cdata = data;
Ranjani Sridharan62004082017-09-06 22:01:40 +0100475 int ret = 0;
Liam Girdwood854b2e52017-09-02 23:14:02 +0100476
Liam Girdwoodbe41b682017-09-21 16:48:18 +0100477 trace_src("cmd");
478
479 ret = comp_set_state(dev, cmd);
480 if (ret < 0)
481 return ret;
Liam Girdwood854b2e52017-09-02 23:14:02 +0100482
Pierre-Louis Bossartb513a452017-09-25 14:52:12 -0500483 if (cmd == COMP_CMD_SET_VALUE)
484 ret = src_ctrl_cmd(dev, cdata);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300485
Ranjani Sridharan62004082017-09-06 22:01:40 +0100486 return ret;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300487}
488
489/* copy and process stream data from source to sink buffers */
490static int src_copy(struct comp_dev *dev)
491{
492 struct comp_data *cd = comp_get_drvdata(dev);
Pierre-Louis Bossart4ccf81d2017-09-25 14:52:09 -0500493 struct comp_buffer *source;
494 struct comp_buffer *sink;
495 int need_source;
496 int need_sink;
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300497 size_t consumed = 0;
498 size_t produced = 0;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300499
Liam Girdwoodab505272017-11-13 16:55:30 +0000500 trace_src("SRC");
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300501
502 /* src component needs 1 source and 1 sink buffer */
503 source = list_first_item(&dev->bsource_list, struct comp_buffer,
504 sink_list);
505 sink = list_first_item(&dev->bsink_list, struct comp_buffer,
506 source_list);
507
Seppo Ingalsuocb3a64d2017-09-06 15:16:12 +0300508 /* Calculate needed amount of source buffer and sink buffer
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300509 * for one SRC run. The blk_in and blk are minimum condition to
510 * call copy. Copy can consume or produce a slightly larger block
511 * with the rates where block sizes are not constant. E.g. for
Pierre-Louis Bossartf9458092017-11-09 15:24:07 -0600512 * 1 ms scheduling the blocks can be under or above 1 ms when the
513 * SRC interval block size constraint prevents exact 1 ms blocks.
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300514 */
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300515 need_source = cd->param.blk_in * dev->frame_bytes;
516 need_sink = cd->param.blk_out * dev->frame_bytes;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300517
Liam Girdwoodab505272017-11-13 16:55:30 +0000518 /* make sure source component buffer has enough data available and that
519 * the sink component buffer has enough free bytes for copy. Also
520 * check for XRUNs */
Seppo Ingalsuo83b75602017-12-05 15:54:57 +0200521 if (source->avail < need_source) {
Liam Girdwoodab505272017-11-13 16:55:30 +0000522 trace_src_error("xru");
523 return -EIO; /* xrun */
524 }
Seppo Ingalsuo83b75602017-12-05 15:54:57 +0200525 if (sink->free < need_sink) {
Liam Girdwoodab505272017-11-13 16:55:30 +0000526 trace_src_error("xro");
527 return -EIO; /* xrun */
528 }
529
Seppo Ingalsuo83b75602017-12-05 15:54:57 +0200530 cd->src_func(dev, source, sink, &consumed, &produced);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300531
Seppo Ingalsuo83b75602017-12-05 15:54:57 +0200532 /* Calc new free and available if data was processed. These
533 * functions must not be called with 0 consumed/produced.
534 */
535 if (consumed > 0)
536 comp_update_buffer_consume(source, consumed);
Seppo Ingalsuo28fafc12017-10-05 18:57:37 +0300537
Liam Girdwoode0f65a22017-12-08 20:34:29 +0000538 if (produced > 0) {
Seppo Ingalsuo83b75602017-12-05 15:54:57 +0200539 comp_update_buffer_produce(sink, produced);
Liam Girdwoode0f65a22017-12-08 20:34:29 +0000540 return cd->param.blk_out;
541 }
Seppo Ingalsuo83b75602017-12-05 15:54:57 +0200542
Liam Girdwoode0f65a22017-12-08 20:34:29 +0000543 /* produced no data */
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300544 return 0;
545}
546
547static int src_prepare(struct comp_dev *dev)
548{
Liam Girdwoodb083e582017-10-12 16:05:44 +0100549 trace_src("pre");
550
551 return comp_set_state(dev, COMP_CMD_PREPARE);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300552}
553
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300554static int src_reset(struct comp_dev *dev)
555{
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300556 struct comp_data *cd = comp_get_drvdata(dev);
557
558 trace_src("SRe");
559
560 cd->src_func = src_2s_s32_default;
Seppo Ingalsuo4b007cb2017-10-20 18:45:30 +0300561 src_polyphase_reset(&cd->src);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300562
Liam Girdwoodb083e582017-10-12 16:05:44 +0100563 comp_set_state(dev, COMP_CMD_RESET);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300564 return 0;
565}
566
567struct comp_driver comp_src = {
568 .type = SOF_COMP_SRC,
Seppo Ingalsuo4a006912017-06-28 19:25:51 +0300569 .ops = {
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300570 .new = src_new,
571 .free = src_free,
572 .params = src_params,
573 .cmd = src_cmd,
574 .copy = src_copy,
575 .prepare = src_prepare,
576 .reset = src_reset,
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300577 },
578};
579
580void sys_comp_src_init(void)
581{
582 comp_register(&comp_src);
583}