blob: c3382206aa191763b68de58d9ea3f2063de8c424 [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 {
58 struct polyphase_src src[PLATFORM_MAX_CHANNELS];
59 int32_t *delay_lines;
60 int scratch_length;
61 //int32_t z[STAGE_BUF_SIZE];
62 void (*src_func)(struct comp_dev *dev,
63 struct comp_buffer *source,
64 struct comp_buffer *sink,
65 uint32_t source_frames,
66 uint32_t sink_frames);
67};
68
69/* Common mute function for 2s and 1s SRC. This preserves the same
70 * buffer consume and produce pattern as normal operation.
71 */
72static void src_muted_s32(struct comp_buffer *source, struct comp_buffer *sink,
73 int blk_in, int blk_out, int nch, int source_frames)
74{
75
76 int i;
77 int32_t *src = (int32_t *) source->r_ptr;
78 int32_t *dest = (int32_t *) sink->w_ptr;
79 int32_t *end = (int32_t *) sink->end_addr;
80 int n_read = 0;
81 int n_max;
82 int n;
83 int n_written = 0;
84
85 for (i = 0; i < source_frames - blk_in + 1; i += blk_in) {
86 n_max = end - dest;
87 n = nch*blk_out;
88 if (n < n_max) {
89 bzero(dest, n * sizeof(int32_t));
90 dest += n;
91 } else {
92 /* Also case n_max == n is done here */
93 bzero(dest, n_max * sizeof(int32_t));
94 dest = (int32_t *) sink->addr;
95 bzero(dest, (n - n_max) * sizeof(int32_t));
96 dest += n - n_max;
97 }
98 n_read += nch*blk_in;
99 n_written += nch*blk_out;
100 }
101 source->r_ptr = src + n_read;
102 sink->w_ptr = dest;
103 comp_wrap_source_r_ptr_circular(source);
104 comp_wrap_sink_w_ptr_circular(sink);
105 comp_update_source_free_avail(source, n_read);
106 comp_update_sink_free_avail(sink, n_written);
107}
108
109/* Fallback function to just output muted samples and advance
110 * pointers. Note that a buffer that is not having integer number of
111 * frames in a period will drift since there is no similar blk in/out
112 * check as for SRC.
113 */
114static void fallback_s32(struct comp_dev *dev,
115 struct comp_buffer *source,
116 struct comp_buffer *sink,
117 uint32_t source_frames,
118 uint32_t sink_frames)
119{
120
121 struct comp_data *cd = comp_get_drvdata(dev);
122 //int32_t *src = (int32_t*) source->r_ptr;
123 //int32_t *dest = (int32_t*) sink->w_ptr;
124 int nch = sink->params.pcm->channels;
125 int blk_in = cd->src[0].blk_in;
126 int blk_out = cd->src[0].blk_out;
127
128 src_muted_s32(source, sink, blk_in, blk_out, nch, source_frames);
129
130}
131
132/* Normal 2 stage SRC */
133static void src_2s_s32_default(struct comp_dev *dev,
134 struct comp_buffer *source, struct comp_buffer *sink,
135 uint32_t source_frames, uint32_t sink_frames)
136{
137 int i, j;
138 struct polyphase_src *s;
139 struct comp_data *cd = comp_get_drvdata(dev);
140 int blk_in = cd->src[0].blk_in;
141 int blk_out = cd->src[0].blk_out;
142 int n_times1 = cd->src[0].stage1_times;
143 int n_times2 = cd->src[0].stage2_times;
144 int nch = sink->params.pcm->channels;
145 int32_t *dest = (int32_t *) sink->w_ptr;
146 int32_t *src = (int32_t *) source->r_ptr;
147 struct src_stage_prm s1, s2;
148 int n_read = 0;
149 int n_written = 0;
150
151 if (cd->src[0].mute) {
152 src_muted_s32(source, sink, blk_in, blk_out, nch, source_frames);
153 return;
154 }
155
156 s1.times = n_times1;
157 s1.x_end_addr = source->end_addr;
158 s1.x_size = source->alloc_size;
159 s1.x_inc = nch;
160 s1.y_end_addr = &cd->delay_lines[cd->scratch_length];
161 s1.y_size = STAGE_BUF_SIZE * sizeof(int32_t);
162 s1.y_inc = 1;
163
164 s2.times = n_times2;
165 s2.x_end_addr = &cd->delay_lines[cd->scratch_length];
166 s2.x_size = STAGE_BUF_SIZE * sizeof(int32_t);
167 s2.x_inc = 1;
168 s2.y_end_addr = sink->end_addr;
169 s2.y_size = sink->alloc_size;
170 s2.y_inc = nch;
171
Seppo Ingalsuo6f27ab62017-06-12 11:31:15 +0300172 s1.x_rptr = src + nch - 1;
173 s2.y_wptr = dest + nch - 1;
174
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300175 for (j = 0; j < nch; j++) {
176 s = &cd->src[j]; /* Point to src[] for this channel */
177 s1.x_rptr = src++;
178 s2.y_wptr = dest++;
179 s1.state = &s->state1;
180 s1.stage = s->stage1;
181 s2.state = &s->state2;
182 s2.stage = s->stage2;
183
184 for (i = 0; i < source_frames - blk_in + 1; i += blk_in) {
185 /* Reset output to buffer start, read interleaved */
186 s1.y_wptr = cd->delay_lines;
187 src_polyphase_stage_cir(&s1);
188 s2.x_rptr = cd->delay_lines;
189 src_polyphase_stage_cir(&s2);
190
191 n_read += blk_in;
192 n_written += blk_out;
193 }
194 }
195 source->r_ptr = s1.x_rptr - nch + 1;
196 sink->w_ptr = s2.y_wptr - nch + 1;
197 comp_wrap_source_r_ptr_circular(source);
198 comp_wrap_sink_w_ptr_circular(sink);
199 comp_update_source_free_avail(source, n_read);
200 comp_update_sink_free_avail(sink, n_written);
201}
202
203/* 1 stage SRC for simple conversions */
204static void src_1s_s32_default(struct comp_dev *dev,
205 struct comp_buffer *source, struct comp_buffer *sink,
206 uint32_t source_frames, uint32_t sink_frames)
207{
208 int i, j;
209 //int32_t *xp, *yp;
210 struct polyphase_src *s;
211
212 struct comp_data *cd = comp_get_drvdata(dev);
213 int blk_in = cd->src[0].blk_in;
214 int blk_out = cd->src[0].blk_out;
215 int n_times = cd->src[0].stage1_times;
216 int nch = sink->params.pcm->channels;
217 int32_t *dest = (int32_t*) sink->w_ptr;
218 int32_t *src = (int32_t*) source->r_ptr;
219 int n_read = 0;
220 int n_written = 0;
221 struct src_stage_prm s1;
222
223 if (cd->src[0].mute) {
224 src_muted_s32(source, sink, blk_in, blk_out, nch, source_frames);
225 return;
226 }
227
228 s1.times = n_times;
229 s1.x_end_addr = source->end_addr;
230 s1.x_size = source->alloc_size;
231 s1.x_inc = nch;
232 s1.y_end_addr = sink->end_addr;
233 s1.y_size = sink->alloc_size;
234 s1.y_inc = nch;
Seppo Ingalsuo6f27ab62017-06-12 11:31:15 +0300235 s1.x_rptr = src + nch - 1;
236 s1.y_wptr = dest + nch - 1;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300237
238 for (j = 0; j < nch; j++) {
239 s = &cd->src[j]; /* Point to src for this channel */
240 s1.x_rptr = src++;
241 s1.y_wptr = dest++;
242 s1.state = &s->state1;
243 s1.stage = s->stage1;
244
245 for (i = 0; i + blk_in - 1 < source_frames; i += blk_in) {
246 src_polyphase_stage_cir(&s1);
247
248 n_read += blk_in;
249 n_written += blk_out;
250 }
251
252 }
253 source->r_ptr = s1.x_rptr - nch + 1;
254 sink->w_ptr = s1.y_wptr - nch + 1;
255
256 comp_wrap_source_r_ptr_circular(source);
257 comp_wrap_sink_w_ptr_circular(sink);
258 comp_update_source_free_avail(source, n_read);
259 comp_update_sink_free_avail(sink, n_written);
260}
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;
266 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;
Liam Girdwood960cf8e2017-06-12 11:38:07 +0100268 int i;
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300269
270 trace_src("SNw");
Liam Girdwood960cf8e2017-06-12 11:38:07 +0100271
Liam Girdwood17641522017-06-09 17:27:02 +0100272 dev = rmalloc(RZONE_RUNTIME, RFLAGS_NONE,
273 COMP_SIZE(struct sof_ipc_comp_src));
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300274 if (dev == NULL)
275 return NULL;
276
Liam Girdwood960cf8e2017-06-12 11:38:07 +0100277 src = (struct sof_ipc_comp_src *)&dev->comp;
278 memcpy(src, ipc_src, sizeof(struct sof_ipc_comp_src));
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300279
280 cd = rmalloc(RZONE_RUNTIME, RFLAGS_NONE, sizeof(*cd));
281 if (cd == NULL) {
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300282 rfree(dev);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300283 return NULL;
284 }
285
286 comp_set_drvdata(dev, cd);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300287
288 cd->delay_lines = NULL;
289 cd->src_func = src_2s_s32_default;
290 for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)
291 src_polyphase_reset(&cd->src[i]);
292
293 return dev;
294}
295
296static void src_free(struct comp_dev *dev)
297{
298 struct comp_data *cd = comp_get_drvdata(dev);
299
300 trace_src("SFr");
301
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300302 /* Free dynamically reserved buffers for SRC algorithm */
303 if (cd->delay_lines != NULL)
304 rfree(cd->delay_lines);
305
306 rfree(cd);
307 rfree(dev);
Seppo Ingalsuo6a274832017-06-07 14:17:55 +0300308}
309
310/* set component audio stream parameters */
311static int src_params(struct comp_dev *dev, struct stream_params *params)
312{
313 int i;
314 struct comp_buffer *source, *sink;
315 struct src_alloc need;
316 size_t delay_lines_size;
317 int32_t *buffer_start;
318 int n = 0;
319 struct comp_data *cd = comp_get_drvdata(dev);
320
321 trace_src("SPa");
322
323 /* SRC supports only S32_LE PCM format */
324 if ((params->type != STREAM_TYPE_PCM)
325 || (params->pcm->frame_fmt != SOF_IPC_FRAME_S32_LE))
326 return -EINVAL;
327
328 /* No data transformation */
329 comp_set_sink_params(dev, params);
330
331 /* Allocate needed memory for delay lines */
332 source = list_first_item(&dev->bsource_list, struct comp_buffer,
333 sink_list);
334 sink = list_first_item(&dev->bsink_list, struct comp_buffer,
335 source_list);
336 src_buffer_lengths(&need, source->params.pcm->rate,
337 sink->params.pcm->rate, source->params.pcm->channels);
338 delay_lines_size = sizeof(int32_t) * need.total;
339 if (cd->delay_lines != NULL)
340 rfree(cd->delay_lines);
341
342 cd->delay_lines = rmalloc(RZONE_RUNTIME, RFLAGS_NONE, delay_lines_size);
343 if (cd->delay_lines == NULL)
344 return -EINVAL;
345
346 /* Clear all delay lines here */
347 memset(cd->delay_lines, 0, delay_lines_size);
348 cd->scratch_length = need.scratch;
349 buffer_start = cd->delay_lines + need.scratch;
350
351 /* Initize SRC for actual sample rate */
352 for (i = 0; i < source->params.pcm->channels; i++) {
353 n = src_polyphase_init(&cd->src[i], source->params.pcm->rate,
354 sink->params.pcm->rate, buffer_start);
355 buffer_start += need.single_src;
356 }
357
358 switch (n) {
359 case 1:
360 cd->src_func = src_1s_s32_default; /* Simpler 1 stage SRC */
361 break;
362 case 2:
363 cd->src_func = src_2s_s32_default; /* Default 2 stage SRC */
364 break;
365 default:
366 /* This is possibly due to missing coefficients for
367 * requested rates combination. Sink audio will be
368 * muted if copy() is run.
369 */
370 trace_src("SFa");
371 cd->src_func = fallback_s32;
372 return(-EINVAL);
373 break;
374 }
375
376 /* Check that src blk_in and blk_out are less than params.period_frames.
377 * Return an error if the period is too short.
378 */
379 if (src_polyphase_get_blk_in(&cd->src[0]) > source->params.pcm->period_count)
380 return(-EINVAL);
381
382 if (src_polyphase_get_blk_out(&cd->src[0]) > sink->params.pcm->period_count)
383 return(-EINVAL);
384
385
386 return 0;
387}
388
389/* used to pass standard and bespoke commands (with data) to component */
390static int src_cmd(struct comp_dev *dev, int cmd, void *data)
391{
392 trace_src("SCm");
393 struct comp_data *cd = comp_get_drvdata(dev);
394 struct sof_ipc_comp_src *cv;
395 int i;
396
397 switch (cmd) {
398 case COMP_CMD_SRC:
399 trace_src("SMa");
400 cv = (struct sof_ipc_comp_src *) data;
401 cv->in_mask = src_input_rates();
402 cv->out_mask = src_output_rates();
403 break;
404 case COMP_CMD_MUTE:
405 trace_src("SMu");
406 for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)
407 src_polyphase_mute(&cd->src[i]);
408
409 break;
410 case COMP_CMD_UNMUTE:
411 trace_src("SUm");
412 for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)
413 src_polyphase_unmute(&cd->src[i]);
414
415 break;
416 case COMP_CMD_START:
417 trace_src("SSt");
418 dev->state = COMP_STATE_RUNNING;
419 break;
420 case COMP_CMD_STOP:
421 trace_src("SSp");
422 if (dev->state == COMP_STATE_RUNNING ||
423 dev->state == COMP_STATE_DRAINING ||
424 dev->state == COMP_STATE_PAUSED) {
425 comp_buffer_reset(dev);
426 dev->state = COMP_STATE_SETUP;
427 }
428 break;
429 case COMP_CMD_PAUSE:
430 trace_src("SPe");
431 /* only support pausing for running */
432 if (dev->state == COMP_STATE_RUNNING)
433 dev->state = COMP_STATE_PAUSED;
434
435 break;
436 case COMP_CMD_RELEASE:
437 trace_src("SRl");
438 dev->state = COMP_STATE_RUNNING;
439 break;
440 default:
441 trace_src("SDf");
442 break;
443 }
444
445 return 0;
446}
447
448/* copy and process stream data from source to sink buffers */
449static int src_copy(struct comp_dev *dev)
450{
451 struct comp_data *cd = comp_get_drvdata(dev);
452 struct comp_buffer *source;
453 struct comp_buffer *sink;
454 uint32_t frames_source;
455 uint32_t frames_sink;
456 int need_source, need_sink, min_frames;
457
458 trace_comp("SRC");
459
460 /* src component needs 1 source and 1 sink buffer */
461 source = list_first_item(&dev->bsource_list, struct comp_buffer,
462 sink_list);
463 sink = list_first_item(&dev->bsink_list, struct comp_buffer,
464 source_list);
465
466 /* Check that source has enough frames available and sink enough
467 * frames free.
468 */
469 frames_source = source->params.pcm->period_count;
470 frames_sink = sink->params.pcm->period_count;
471 min_frames = src_polyphase_get_blk_in(&cd->src[0]);
472 if (frames_source > min_frames)
473 need_source = frames_source * source->params.pcm->frame_size;
474 else {
475 frames_source = min_frames;
476 need_source = min_frames * source->params.pcm->frame_size;
477 }
478
479 min_frames = src_polyphase_get_blk_out(&cd->src[0]);
480 if (frames_sink > min_frames)
481 need_sink = frames_sink * sink->params.pcm->frame_size;
482 else {
483 frames_sink = min_frames;
484 need_sink = min_frames * sink->params.pcm->frame_size;
485 }
486
487 /* Run as many times as buffers allow */
488 while ((source->avail >= need_source) && (sink->free >= need_sink)) {
489 /* Run src */
490 cd->src_func(dev, source, sink, frames_source, frames_sink);
491
492 }
493
494 return 0;
495}
496
497static int src_prepare(struct comp_dev *dev)
498{
499 // struct comp_data *cd = comp_get_drvdata(dev);
500 struct comp_buffer *source;
501 struct comp_buffer *sink;
502 // int i;
503
504 trace_src("SPp");
505
506#if 1
507 source = list_first_item(&dev->bsource_list, struct comp_buffer,
508 sink_list);
509 sink = list_first_item(&dev->bsink_list, struct comp_buffer,
510 source_list);
511
512 trace_value(source->params.pcm->channels);
513 trace_value(source->params.pcm->rate);
514 trace_value(sink->params.pcm->rate);
515#endif
516
517 //dev->preload = PLAT_INT_PERIODS;
518 dev->state = COMP_STATE_PREPARE;
519 return 0;
520}
521
522static int src_preload(struct comp_dev *dev)
523{
524 //int i;
525 trace_src("SPl");
526
527
528 //for (i = 0; i < dev->preload; i++)
529 // src_copy(dev);
530
531 return 0;
532}
533
534static int src_reset(struct comp_dev *dev)
535{
536 int i;
537 struct comp_data *cd = comp_get_drvdata(dev);
538
539 trace_src("SRe");
540
541 cd->src_func = src_2s_s32_default;
542 for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)
543 src_polyphase_reset(&cd->src[i]);
544
545 dev->state = COMP_STATE_INIT;
546 return 0;
547}
548
549struct comp_driver comp_src = {
550 .type = SOF_COMP_SRC,
551 .ops =
552 {
553 .new = src_new,
554 .free = src_free,
555 .params = src_params,
556 .cmd = src_cmd,
557 .copy = src_copy,
558 .prepare = src_prepare,
559 .reset = src_reset,
560 .preload = src_preload,
561 },
562};
563
564void sys_comp_src_init(void)
565{
566 comp_register(&comp_src);
567}