1 // SPDX-License-Identifier: BSD-3-Clause
2 //
3 // Copyright(c) 2017 Intel Corporation. All rights reserved.
4 //
5 // Author: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>
6 // Liam Girdwood <liam.r.girdwood@linux.intel.com>
7 // Keyon Jie <yang.jie@linux.intel.com>
8
9 #include <sof/common.h>
10 #include <sof/audio/buffer.h>
11 #include <sof/audio/component.h>
12 #include <sof/audio/pipeline.h>
13 #include <sof/audio/ipc-config.h>
14 #include <sof/audio/src/src.h>
15 #include <sof/audio/src/src_config.h>
16 #include <sof/debug/panic.h>
17 #include <sof/ipc/msg.h>
18 #include <sof/lib/alloc.h>
19 #include <sof/lib/memory.h>
20 #include <sof/lib/uuid.h>
21 #include <sof/list.h>
22 #include <sof/math/numbers.h>
23 #include <sof/platform.h>
24 #include <sof/string.h>
25 #include <sof/ut.h>
26 #include <sof/trace/trace.h>
27 #include <ipc/control.h>
28 #include <ipc/stream.h>
29 #include <ipc/topology.h>
30 #include <user/trace.h>
31 #include <errno.h>
32 #include <stddef.h>
33 #include <stdint.h>
34
35 #if SRC_SHORT || CONFIG_COMP_SRC_TINY
36 #include <sof/audio/coefficients/src/src_tiny_int16_define.h>
37 #include <sof/audio/coefficients/src/src_tiny_int16_table.h>
38 #else
39 #if CONFIG_COMP_SRC_SMALL
40 #include <sof/audio/coefficients/src/src_small_int32_define.h>
41 #include <sof/audio/coefficients/src/src_small_int32_table.h>
42 #endif
43 #if CONFIG_COMP_SRC_STD
44 #include <sof/audio/coefficients/src/src_std_int32_define.h>
45 #include <sof/audio/coefficients/src/src_std_int32_table.h>
46 #endif
47 #endif
48
49 /* The FIR maximum lengths are per channel so need to multiply them */
50 #define MAX_FIR_DELAY_SIZE_XNCH (PLATFORM_MAX_CHANNELS * MAX_FIR_DELAY_SIZE)
51 #define MAX_OUT_DELAY_SIZE_XNCH (PLATFORM_MAX_CHANNELS * MAX_OUT_DELAY_SIZE)
52
53 static const struct comp_driver comp_src;
54
55 /* c1c5326d-8390-46b4-aa47-95c3beca6550 */
56 DECLARE_SOF_RT_UUID("src", src_uuid, 0xc1c5326d, 0x8390, 0x46b4,
57 0xaa, 0x47, 0x95, 0xc3, 0xbe, 0xca, 0x65, 0x50);
58
59 DECLARE_TR_CTX(src_tr, SOF_UUID(src_uuid), LOG_LEVEL_INFO);
60
61 /* src component private data */
62 struct comp_data {
63 struct polyphase_src src;
64 struct src_param param;
65 struct ipc_config_src ipc_config;
66 int32_t *delay_lines;
67 uint32_t sink_rate;
68 uint32_t source_rate;
69 int32_t *sbuf_w_ptr;
70 int32_t *sbuf_r_ptr;
71 int sbuf_avail;
72 int data_shift;
73 int source_frames;
74 int sink_frames;
75 int sample_container_bytes;
76 void (*src_func)(struct comp_dev *dev,
77 const struct audio_stream *source,
78 struct audio_stream *sink,
79 int *consumed,
80 int *produced);
81 void (*polyphase_func)(struct src_stage_prm *s);
82 };
83
84 /* Calculates the needed FIR delay line length */
src_fir_delay_length(struct src_stage * s)85 static int src_fir_delay_length(struct src_stage *s)
86 {
87 return s->subfilter_length + (s->num_of_subfilters - 1) * s->idm
88 + s->blk_in;
89 }
90
91 /* Calculates the FIR output delay line length */
src_out_delay_length(struct src_stage * s)92 static int src_out_delay_length(struct src_stage *s)
93 {
94 return 1 + (s->num_of_subfilters - 1) * s->odm;
95 }
96
97 /* Returns index of a matching sample rate */
src_find_fs(int fs_list[],int list_length,int fs)98 static int src_find_fs(int fs_list[], int list_length, int fs)
99 {
100 int i;
101
102 for (i = 0; i < list_length; i++) {
103 if (fs_list[i] == fs)
104 return i;
105 }
106 return -EINVAL;
107 }
108
109 /* Calculates buffers to allocate for a SRC mode */
src_buffer_lengths(struct src_param * a,int fs_in,int fs_out,int nch,int source_frames)110 int src_buffer_lengths(struct src_param *a, int fs_in, int fs_out, int nch,
111 int source_frames)
112 {
113 struct src_stage *stage1;
114 struct src_stage *stage2;
115 int r1;
116
117 if (nch > PLATFORM_MAX_CHANNELS) {
118 /* TODO: should be device, not class */
119 comp_cl_err(&comp_src, "src_buffer_lengths(): nch = %u > PLATFORM_MAX_CHANNELS",
120 nch);
121 return -EINVAL;
122 }
123
124 a->nch = nch;
125 a->idx_in = src_find_fs(src_in_fs, NUM_IN_FS, fs_in);
126 a->idx_out = src_find_fs(src_out_fs, NUM_OUT_FS, fs_out);
127
128 /* Check that both in and out rates are supported */
129 if (a->idx_in < 0 || a->idx_out < 0) {
130 comp_cl_err(&comp_src, "src_buffer_lengths(): rates not supported, fs_in: %u, fs_out: %u",
131 fs_in, fs_out);
132 return -EINVAL;
133 }
134
135 stage1 = src_table1[a->idx_out][a->idx_in];
136 stage2 = src_table2[a->idx_out][a->idx_in];
137
138 /* Check from stage1 parameter for a deleted in/out rate combination.*/
139 if (stage1->filter_length < 1) {
140 comp_cl_err(&comp_src, "src_buffer_lengths(): Non-supported combination sfs_in = %d, fs_out = %d",
141 fs_in, fs_out);
142 return -EINVAL;
143 }
144
145 a->fir_s1 = nch * src_fir_delay_length(stage1);
146 a->out_s1 = nch * src_out_delay_length(stage1);
147
148 /* Computing of number of blocks to process is done in
149 * copy() per each frame.
150 */
151 a->stage1_times = 0;
152 a->stage2_times = 0;
153 a->blk_in = 0;
154 a->blk_out = 0;
155
156 if (stage2->filter_length == 1) {
157 a->fir_s2 = 0;
158 a->out_s2 = 0;
159 a->sbuf_length = 0;
160 } else {
161 a->fir_s2 = nch * src_fir_delay_length(stage2);
162 a->out_s2 = nch * src_out_delay_length(stage2);
163
164 /* Stage 1 is repeated max. amount that just exceeds one
165 * period.
166 */
167 r1 = source_frames / stage1->blk_in + 1;
168
169 /* Set sbuf length to allow storing two stage 1 output
170 * periods. This is an empirically found value for no
171 * xruns to happen with SRC in/out buffers. Due to
172 * variable number of blocks to process per each stage
173 * there is no equation known for minimum size.
174 */
175 a->sbuf_length = 2 * nch * stage1->blk_out * r1;
176 }
177
178 a->src_multich = a->fir_s1 + a->fir_s2 + a->out_s1 + a->out_s2;
179 a->total = a->sbuf_length + a->src_multich;
180
181 return 0;
182 }
183
src_state_reset(struct src_state * state)184 static void src_state_reset(struct src_state *state)
185 {
186 state->fir_delay_size = 0;
187 state->out_delay_size = 0;
188 }
189
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)190 static int init_stages(struct src_stage *stage1, struct src_stage *stage2,
191 struct polyphase_src *src, struct src_param *p,
192 int n, int32_t *delay_lines_start)
193 {
194 /* Clear FIR state */
195 src_state_reset(&src->state1);
196 src_state_reset(&src->state2);
197
198 src->number_of_stages = n;
199 src->stage1 = stage1;
200 src->stage2 = stage2;
201 if (n == 1 && stage1->blk_out == 0)
202 return -EINVAL;
203
204 /* Optimized SRC requires subfilter length multiple of 4 */
205 if (stage1->filter_length > 1 && (stage1->subfilter_length & 0x3) > 0)
206 return -EINVAL;
207
208 if (stage2->filter_length > 1 && (stage2->subfilter_length & 0x3) > 0)
209 return -EINVAL;
210
211 /* Delay line sizes */
212 src->state1.fir_delay_size = p->fir_s1;
213 src->state1.out_delay_size = p->out_s1;
214 src->state1.fir_delay = delay_lines_start;
215 src->state1.out_delay =
216 src->state1.fir_delay + src->state1.fir_delay_size;
217 /* Initialize to last ensures that circular wrap cannot happen
218 * mid-frame. The size is multiple of channels count.
219 */
220 src->state1.fir_wp = &src->state1.fir_delay[p->fir_s1 - 1];
221 src->state1.out_rp = src->state1.out_delay;
222 if (n > 1) {
223 src->state2.fir_delay_size = p->fir_s2;
224 src->state2.out_delay_size = p->out_s2;
225 src->state2.fir_delay =
226 src->state1.out_delay + src->state1.out_delay_size;
227 src->state2.out_delay =
228 src->state2.fir_delay + src->state2.fir_delay_size;
229 /* Initialize to last ensures that circular wrap cannot happen
230 * mid-frame. The size is multiple of channels count.
231 */
232 src->state2.fir_wp = &src->state2.fir_delay[p->fir_s2 - 1];
233 src->state2.out_rp = src->state2.out_delay;
234 } else {
235 src->state2.fir_delay_size = 0;
236 src->state2.out_delay_size = 0;
237 src->state2.fir_delay = NULL;
238 src->state2.out_delay = NULL;
239 }
240
241 /* Check the sizes are less than MAX */
242 if (src->state1.fir_delay_size > MAX_FIR_DELAY_SIZE_XNCH ||
243 src->state1.out_delay_size > MAX_OUT_DELAY_SIZE_XNCH ||
244 src->state2.fir_delay_size > MAX_FIR_DELAY_SIZE_XNCH ||
245 src->state2.out_delay_size > MAX_OUT_DELAY_SIZE_XNCH) {
246 src->state1.fir_delay = NULL;
247 src->state1.out_delay = NULL;
248 src->state2.fir_delay = NULL;
249 src->state2.out_delay = NULL;
250 return -EINVAL;
251 }
252
253 return 0;
254 }
255
src_polyphase_reset(struct polyphase_src * src)256 void src_polyphase_reset(struct polyphase_src *src)
257 {
258 src->number_of_stages = 0;
259 src->stage1 = NULL;
260 src->stage2 = NULL;
261 src_state_reset(&src->state1);
262 src_state_reset(&src->state2);
263 }
264
src_polyphase_init(struct polyphase_src * src,struct src_param * p,int32_t * delay_lines_start)265 int src_polyphase_init(struct polyphase_src *src, struct src_param *p,
266 int32_t *delay_lines_start)
267 {
268 struct src_stage *stage1;
269 struct src_stage *stage2;
270 int n_stages;
271 int ret;
272
273 if (p->idx_in < 0 || p->idx_out < 0)
274 return -EINVAL;
275
276 /* Get setup for 2 stage conversion */
277 stage1 = src_table1[p->idx_out][p->idx_in];
278 stage2 = src_table2[p->idx_out][p->idx_in];
279 ret = init_stages(stage1, stage2, src, p, 2, delay_lines_start);
280 if (ret < 0)
281 return -EINVAL;
282
283 /* Get number of stages used for optimize opportunity. 2nd
284 * stage length is one if conversion needs only one stage.
285 * If input and output rate is the same return 0 to
286 * use a simple copy function instead of 1 stage FIR with one
287 * tap.
288 */
289 n_stages = (src->stage2->filter_length == 1) ? 1 : 2;
290 if (p->idx_in == p->idx_out)
291 n_stages = 0;
292
293 /* If filter length for first stage is zero this is a deleted
294 * mode from in/out matrix. Computing of such SRC mode needs
295 * to be prevented.
296 */
297 if (src->stage1->filter_length == 0)
298 return -EINVAL;
299
300 return n_stages;
301 }
302
303 /* Fallback function */
src_fallback(struct comp_dev * dev,const struct audio_stream * source,struct audio_stream * sink,int * n_read,int * n_written)304 static void src_fallback(struct comp_dev *dev,
305 const struct audio_stream *source,
306 struct audio_stream *sink, int *n_read, int *n_written)
307 {
308 *n_read = 0;
309 *n_written = 0;
310 }
311
312 /* Normal 2 stage SRC */
src_2s(struct comp_dev * dev,const struct audio_stream * source,struct audio_stream * sink,int * n_read,int * n_written)313 static void src_2s(struct comp_dev *dev, const struct audio_stream *source,
314 struct audio_stream *sink, int *n_read, int *n_written)
315 {
316 struct src_stage_prm s1;
317 struct src_stage_prm s2;
318 int s1_blk_in;
319 int s1_blk_out;
320 int s2_blk_in;
321 int s2_blk_out;
322 struct comp_data *cd = comp_get_drvdata(dev);
323 void *sbuf_addr = cd->delay_lines;
324 void *sbuf_end_addr = &cd->delay_lines[cd->param.sbuf_length];
325 size_t sbuf_size = cd->param.sbuf_length * sizeof(int32_t);
326 int nch = source->channels;
327 int sbuf_free = cd->param.sbuf_length - cd->sbuf_avail;
328 int avail_b = audio_stream_get_avail_bytes(source);
329 int free_b = audio_stream_get_free_bytes(sink);
330 int sz = cd->sample_container_bytes;
331
332 *n_read = 0;
333 *n_written = 0;
334 s1.x_end_addr = source->end_addr;
335 s1.x_size = source->size;
336 s1.y_addr = sbuf_addr;
337 s1.y_end_addr = sbuf_end_addr;
338 s1.y_size = sbuf_size;
339 s1.state = &cd->src.state1;
340 s1.stage = cd->src.stage1;
341 s1.x_rptr = source->r_ptr;
342 s1.y_wptr = cd->sbuf_w_ptr;
343 s1.nch = nch;
344 s1.shift = cd->data_shift;
345
346 s2.x_end_addr = sbuf_end_addr;
347 s2.x_size = sbuf_size;
348 s2.y_addr = sink->addr;
349 s2.y_end_addr = sink->end_addr;
350 s2.y_size = sink->size;
351 s2.state = &cd->src.state2;
352 s2.stage = cd->src.stage2;
353 s2.x_rptr = cd->sbuf_r_ptr;
354 s2.y_wptr = sink->w_ptr;
355 s2.nch = nch;
356 s2.shift = cd->data_shift;
357
358 /* Test if 1st stage can be run with default block length to reach
359 * the period length or just under it.
360 */
361 s1.times = cd->param.stage1_times;
362 s1_blk_in = s1.times * cd->src.stage1->blk_in * nch;
363 s1_blk_out = s1.times * cd->src.stage1->blk_out * nch;
364
365 /* The sbuf may limit how many times s1 can be looped. It's harder
366 * to prepare for in advance so the repeats number is adjusted down
367 * here if need.
368 */
369 if (s1_blk_out > sbuf_free) {
370 s1.times = sbuf_free / (cd->src.stage1->blk_out * nch);
371 s1_blk_in = s1.times * cd->src.stage1->blk_in * nch;
372 s1_blk_out = s1.times * cd->src.stage1->blk_out * nch;
373 comp_dbg(dev, "s1.times = %d", s1.times);
374 }
375
376 if (avail_b >= s1_blk_in * sz && sbuf_free >= s1_blk_out) {
377 cd->polyphase_func(&s1);
378
379 cd->sbuf_w_ptr = s1.y_wptr;
380 cd->sbuf_avail += s1_blk_out;
381 *n_read += s1.times * cd->src.stage1->blk_in;
382 }
383
384 s2.times = cd->param.stage2_times;
385 s2_blk_in = s2.times * cd->src.stage2->blk_in * nch;
386 s2_blk_out = s2.times * cd->src.stage2->blk_out * nch;
387 if (s2_blk_in > cd->sbuf_avail) {
388 s2.times = cd->sbuf_avail / (cd->src.stage2->blk_in * nch);
389 s2_blk_in = s2.times * cd->src.stage2->blk_in * nch;
390 s2_blk_out = s2.times * cd->src.stage2->blk_out * nch;
391 comp_dbg(dev, "s2.times = %d", s2.times);
392 }
393
394 /* Test if second stage can be run with default block length. */
395 if (cd->sbuf_avail >= s2_blk_in && free_b >= s2_blk_out * sz) {
396 cd->polyphase_func(&s2);
397
398 cd->sbuf_r_ptr = s2.x_rptr;
399 cd->sbuf_avail -= s2_blk_in;
400 *n_written += s2.times * cd->src.stage2->blk_out;
401 }
402 }
403
404 /* 1 stage SRC for simple conversions */
src_1s(struct comp_dev * dev,const struct audio_stream * source,struct audio_stream * sink,int * n_read,int * n_written)405 static void src_1s(struct comp_dev *dev, const struct audio_stream *source,
406 struct audio_stream *sink, int *n_read, int *n_written)
407 {
408 struct src_stage_prm s1;
409 struct comp_data *cd = comp_get_drvdata(dev);
410
411 s1.times = cd->param.stage1_times;
412 s1.x_rptr = source->r_ptr;
413 s1.x_end_addr = source->end_addr;
414 s1.x_size = source->size;
415 s1.y_wptr = sink->w_ptr;
416 s1.y_end_addr = sink->end_addr;
417 s1.y_size = sink->size;
418 s1.state = &cd->src.state1;
419 s1.stage = cd->src.stage1;
420 s1.nch = source->channels;
421 s1.shift = cd->data_shift;
422
423 cd->polyphase_func(&s1);
424
425 *n_read = cd->param.blk_in;
426 *n_written = cd->param.blk_out;
427 }
428
429 /* A fast copy function for same in and out rate */
src_copy_sxx(struct comp_dev * dev,const struct audio_stream * source,struct audio_stream * sink,int * n_read,int * n_written)430 static void src_copy_sxx(struct comp_dev *dev,
431 const struct audio_stream *source,
432 struct audio_stream *sink,
433 int *n_read, int *n_written)
434 {
435 struct comp_data *cd = comp_get_drvdata(dev);
436 int frames = cd->param.blk_in;
437
438 switch (sink->frame_fmt) {
439 case SOF_IPC_FRAME_S16_LE:
440 case SOF_IPC_FRAME_S24_4LE:
441 case SOF_IPC_FRAME_S32_LE:
442 audio_stream_copy(source, 0, sink, 0,
443 frames * source->channels);
444 *n_read = frames;
445 *n_written = frames;
446 break;
447 default:
448 *n_read = 0;
449 *n_written = 0;
450 }
451 }
452
src_new(const struct comp_driver * drv,struct comp_ipc_config * config,void * spec)453 static struct comp_dev *src_new(const struct comp_driver *drv,
454 struct comp_ipc_config *config,
455 void *spec)
456 {
457 struct comp_dev *dev;
458 struct comp_data *cd;
459 struct ipc_config_src *ipc_src = spec;
460
461 comp_cl_info(&comp_src, "src_new()");
462
463 /* validate init data - either SRC sink or source rate must be set */
464 if (ipc_src->source_rate == 0 && ipc_src->sink_rate == 0) {
465 comp_cl_err(&comp_src, "src_new(): SRC sink and source rate are not set");
466 return NULL;
467 }
468
469 dev = comp_alloc(drv, sizeof(*dev));
470 if (!dev)
471 return NULL;
472 dev->ipc_config = *config;
473
474 cd = rzalloc(SOF_MEM_ZONE_RUNTIME, 0, SOF_MEM_CAPS_RAM, sizeof(*cd));
475 if (!cd) {
476 rfree(dev);
477 return NULL;
478 }
479
480 comp_set_drvdata(dev, cd);
481 cd->ipc_config = *ipc_src;
482
483 cd->delay_lines = NULL;
484 cd->src_func = src_fallback;
485 cd->polyphase_func = NULL;
486 src_polyphase_reset(&cd->src);
487
488 dev->state = COMP_STATE_READY;
489 return dev;
490 }
491
src_free(struct comp_dev * dev)492 static void src_free(struct comp_dev *dev)
493 {
494 struct comp_data *cd = comp_get_drvdata(dev);
495
496 comp_info(dev, "src_free()");
497
498 /* Free dynamically reserved buffers for SRC algorithm */
499 if (cd->delay_lines)
500 rfree(cd->delay_lines);
501
502 rfree(cd);
503 rfree(dev);
504 }
505
src_verify_params(struct comp_dev * dev,struct sof_ipc_stream_params * params)506 static int src_verify_params(struct comp_dev *dev,
507 struct sof_ipc_stream_params *params)
508 {
509 struct comp_data *cd = comp_get_drvdata(dev);
510 int ret;
511
512 comp_dbg(dev, "src_verify_params()");
513
514 /* check whether params->rate (received from driver) are equal
515 * to src->source_rate (PLAYBACK) or src->sink_rate (CAPTURE) set during
516 * creating src component in src_new().
517 * src->source/sink_rate = 0 means that source/sink rate can vary.
518 */
519 if (dev->direction == SOF_IPC_STREAM_PLAYBACK) {
520 if (cd->ipc_config.source_rate && (params->rate != cd->ipc_config.source_rate)) {
521 comp_err(dev, "src_verify_params(): runtime stream pcm rate %u does not match rate %u fetched from ipc.",
522 params->rate, cd->ipc_config.source_rate);
523 return -EINVAL;
524 }
525 } else {
526 if (cd->ipc_config.sink_rate && (params->rate != cd->ipc_config.sink_rate)) {
527 comp_err(dev, "src_verify_params(): runtime stream pcm rate %u does not match rate %u fetched from ipc.",
528 params->rate, cd->ipc_config.sink_rate);
529 return -EINVAL;
530 }
531 }
532
533 /* update downstream (playback) or upstream (capture) buffer parameters
534 */
535 ret = comp_verify_params(dev, BUFF_PARAMS_RATE, params);
536 if (ret < 0) {
537 comp_err(dev, "src_verify_params(): comp_verify_params() failed.");
538 return ret;
539 }
540
541 return 0;
542 }
543
544 /* set component audio stream parameters */
src_params(struct comp_dev * dev,struct sof_ipc_stream_params * params)545 static int src_params(struct comp_dev *dev,
546 struct sof_ipc_stream_params *params)
547 {
548 struct comp_data *cd = comp_get_drvdata(dev);
549 struct comp_buffer *sinkb;
550 struct comp_buffer *sourceb;
551 size_t delay_lines_size;
552 int32_t *buffer_start;
553 int n = 0;
554 int err;
555
556 comp_info(dev, "src_params()");
557
558 err = src_verify_params(dev, params);
559 if (err < 0) {
560 comp_err(dev, "src_params(): pcm params verification failed.");
561 return -EINVAL;
562 }
563
564 cd->sample_container_bytes = params->sample_container_bytes;
565
566 /* src components will only ever have 1 source and 1 sink buffer */
567 sourceb = list_first_item(&dev->bsource_list, struct comp_buffer,
568 sink_list);
569 sinkb = list_first_item(&dev->bsink_list, struct comp_buffer,
570 source_list);
571
572 comp_info(dev, "src_params(): src->source_rate: %d", cd->ipc_config.source_rate);
573 comp_info(dev, "src_params(): src->sink_rate: %d", cd->ipc_config.sink_rate);
574
575 /* Set source/sink_rate/frames */
576 cd->source_rate = sourceb->stream.rate;
577 cd->sink_rate = sinkb->stream.rate;
578 if (!cd->sink_rate) {
579 comp_err(dev, "src_params(), zero sink rate");
580 return -EINVAL;
581 }
582
583 cd->source_frames = dev->frames * cd->source_rate / cd->sink_rate;
584 cd->sink_frames = dev->frames;
585
586 /* Allocate needed memory for delay lines */
587 comp_info(dev, "src_params(), source_rate = %u, sink_rate = %u",
588 cd->source_rate, cd->sink_rate);
589 comp_info(dev, "src_params(), sourceb->channels = %u, sinkb->channels = %u, dev->frames = %u",
590 sourceb->stream.channels,
591 sinkb->stream.channels, dev->frames);
592 err = src_buffer_lengths(&cd->param, cd->source_rate,
593 cd->sink_rate,
594 sourceb->stream.channels, cd->source_frames);
595 if (err < 0) {
596 comp_err(dev, "src_params(): src_buffer_lengths() failed");
597 return err;
598 }
599
600 comp_info(dev, "src_params(), blk_in = %u, blk_out = %u",
601 cd->param.blk_in, cd->param.blk_out);
602
603 delay_lines_size = sizeof(int32_t) * cd->param.total;
604 if (delay_lines_size == 0) {
605 comp_err(dev, "src_params(): delay_lines_size = 0");
606 return -EINVAL;
607 }
608
609 /* free any existing delay lines. TODO reuse if same size */
610 if (cd->delay_lines)
611 rfree(cd->delay_lines);
612
613 cd->delay_lines = rballoc(0, SOF_MEM_CAPS_RAM, delay_lines_size);
614 if (!cd->delay_lines) {
615 comp_err(dev, "src_params(): failed to alloc cd->delay_lines, delay_lines_size = %u",
616 delay_lines_size);
617 return -EINVAL;
618 }
619
620 /* Clear all delay lines here */
621 memset(cd->delay_lines, 0, delay_lines_size);
622 buffer_start = cd->delay_lines + cd->param.sbuf_length;
623
624 /* Initialize SRC for actual sample rate */
625 n = src_polyphase_init(&cd->src, &cd->param, buffer_start);
626
627 /* Reset stage buffer */
628 cd->sbuf_r_ptr = cd->delay_lines;
629 cd->sbuf_w_ptr = cd->delay_lines;
630 cd->sbuf_avail = 0;
631
632 switch (n) {
633 case 0:
634 /* 1:1 fast copy */
635 cd->src_func = src_copy_sxx;
636 break;
637 case 1:
638 cd->src_func = src_1s; /* Simpler 1 stage SRC */
639 break;
640 case 2:
641 cd->src_func = src_2s; /* Default 2 stage SRC */
642 break;
643 default:
644 /* This is possibly due to missing coefficients for
645 * requested rates combination.
646 */
647 comp_info(dev, "src_params(), missing coefficients for requested rates combination");
648 cd->src_func = src_fallback;
649 return -EINVAL;
650 }
651
652 return 0;
653 }
654
src_ctrl_cmd(struct comp_dev * dev,struct sof_ipc_ctrl_data * cdata)655 static int src_ctrl_cmd(struct comp_dev *dev, struct sof_ipc_ctrl_data *cdata)
656 {
657 comp_err(dev, "src_ctrl_cmd()");
658 return -EINVAL;
659 }
660
661 /* used to pass standard and bespoke commands (with data) to component */
src_cmd(struct comp_dev * dev,int cmd,void * data,int max_data_size)662 static int src_cmd(struct comp_dev *dev, int cmd, void *data,
663 int max_data_size)
664 {
665 struct sof_ipc_ctrl_data *cdata = ASSUME_ALIGNED(data, 4);
666 int ret = 0;
667
668 comp_info(dev, "src_cmd()");
669
670 if (cmd == COMP_CMD_SET_VALUE)
671 ret = src_ctrl_cmd(dev, cdata);
672
673 return ret;
674 }
675
src_trigger(struct comp_dev * dev,int cmd)676 static int src_trigger(struct comp_dev *dev, int cmd)
677 {
678 struct comp_data *cd = comp_get_drvdata(dev);
679
680 comp_info(dev, "src_trigger()");
681
682 if (cmd == COMP_TRIGGER_START || cmd == COMP_TRIGGER_RELEASE)
683 assert(cd->polyphase_func);
684
685 return comp_set_state(dev, cmd);
686 }
687
src_get_copy_limits(struct comp_data * cd,const struct comp_buffer * source,const struct comp_buffer * sink)688 static int src_get_copy_limits(struct comp_data *cd,
689 const struct comp_buffer *source,
690 const struct comp_buffer *sink)
691 {
692 struct src_param *sp;
693 struct src_stage *s1;
694 struct src_stage *s2;
695 int frames_src;
696 int frames_snk;
697
698 /* Get SRC parameters */
699 sp = &cd->param;
700 s1 = cd->src.stage1;
701 s2 = cd->src.stage2;
702
703 /* Calculate how many blocks can be processed with
704 * available source and free sink frames amount.
705 */
706 if (s2->filter_length > 1) {
707 /* Two polyphase filters case */
708 frames_snk = audio_stream_get_free_frames(&sink->stream);
709 frames_snk = MIN(frames_snk, cd->sink_frames + s2->blk_out);
710 sp->stage2_times = frames_snk / s2->blk_out;
711 frames_src = audio_stream_get_avail_frames(&source->stream);
712 frames_src = MIN(frames_src, cd->source_frames + s1->blk_in);
713 sp->stage1_times = frames_src / s1->blk_in;
714 sp->blk_in = sp->stage1_times * s1->blk_in;
715 sp->blk_out = sp->stage2_times * s2->blk_out;
716 } else {
717 /* Single polyphase filter case */
718 frames_snk = audio_stream_get_free_frames(&sink->stream);
719 frames_snk = MIN(frames_snk, cd->sink_frames + s1->blk_out);
720 sp->stage1_times = frames_snk / s1->blk_out;
721 frames_src = audio_stream_get_avail_frames(&source->stream);
722 sp->stage1_times = MIN(sp->stage1_times,
723 frames_src / s1->blk_in);
724 sp->blk_in = sp->stage1_times * s1->blk_in;
725 sp->blk_out = sp->stage1_times * s1->blk_out;
726 }
727
728 if (sp->blk_in == 0 || sp->blk_out == 0)
729 return -EIO;
730
731 return 0;
732 }
733
src_process(struct comp_dev * dev,struct comp_buffer * source,struct comp_buffer * sink)734 static void src_process(struct comp_dev *dev, struct comp_buffer *source,
735 struct comp_buffer *sink)
736 {
737 struct comp_data *cd = comp_get_drvdata(dev);
738 int consumed = 0;
739 int produced = 0;
740
741 /* consumed bytes are not known at this point */
742 buffer_invalidate(source, source->stream.size);
743 cd->src_func(dev, &source->stream, &sink->stream, &consumed, &produced);
744 buffer_writeback(sink, produced *
745 audio_stream_frame_bytes(&sink->stream));
746
747 comp_dbg(dev, "src_copy(), consumed = %u, produced = %u",
748 consumed, produced);
749
750 comp_update_buffer_consume(source, consumed *
751 audio_stream_frame_bytes(&source->stream));
752 comp_update_buffer_produce(sink, produced *
753 audio_stream_frame_bytes(&sink->stream));
754 }
755
756 /* copy and process stream data from source to sink buffers */
src_copy(struct comp_dev * dev)757 static int src_copy(struct comp_dev *dev)
758 {
759 struct comp_data *cd = comp_get_drvdata(dev);
760 struct comp_buffer *source;
761 struct comp_buffer *sink;
762 int ret;
763 uint32_t flags = 0;
764
765 comp_dbg(dev, "src_copy()");
766
767 /* src component needs 1 source and 1 sink buffer */
768 source = list_first_item(&dev->bsource_list, struct comp_buffer,
769 sink_list);
770 sink = list_first_item(&dev->bsink_list, struct comp_buffer,
771 source_list);
772
773 buffer_lock(source, &flags);
774 buffer_lock(sink, &flags);
775
776 /* Get from buffers and SRC conversion specific block constraints
777 * how many frames can be processed. If sufficient number of samples
778 * is not available the processing is omitted.
779 */
780 ret = src_get_copy_limits(cd, source, sink);
781
782 buffer_unlock(sink, flags);
783 buffer_unlock(source, flags);
784
785 if (ret) {
786 comp_info(dev, "No data to process.");
787 return PPL_STATUS_PATH_STOP;
788 }
789
790 src_process(dev, source, sink);
791
792 /* produced no data */
793 return 0;
794 }
795
src_prepare(struct comp_dev * dev)796 static int src_prepare(struct comp_dev *dev)
797 {
798 struct comp_data *cd = comp_get_drvdata(dev);
799 struct comp_buffer *sinkb;
800 struct comp_buffer *sourceb;
801 enum sof_ipc_frame source_format;
802 enum sof_ipc_frame sink_format;
803 uint32_t source_period_bytes;
804 uint32_t sink_period_bytes;
805 int ret;
806
807 comp_info(dev, "src_prepare()");
808
809 ret = comp_set_state(dev, COMP_TRIGGER_PREPARE);
810 if (ret < 0)
811 return ret;
812
813 if (ret == COMP_STATUS_STATE_ALREADY_SET)
814 return PPL_STATUS_PATH_STOP;
815
816 /* SRC component will only ever have 1 source and 1 sink buffer */
817 sourceb = list_first_item(&dev->bsource_list,
818 struct comp_buffer, sink_list);
819 sinkb = list_first_item(&dev->bsink_list,
820 struct comp_buffer, source_list);
821
822 /* get source data format and period bytes */
823 source_format = sourceb->stream.frame_fmt;
824 source_period_bytes = audio_stream_period_bytes(&sourceb->stream,
825 dev->frames);
826
827 /* get sink data format and period bytes */
828 sink_format = sinkb->stream.frame_fmt;
829 sink_period_bytes = audio_stream_period_bytes(&sinkb->stream,
830 dev->frames);
831
832 if (sinkb->stream.size < dev->ipc_config.periods_sink * sink_period_bytes) {
833 comp_err(dev, "src_prepare(): sink buffer size %d is insufficient < %d * %d",
834 sinkb->stream.size, dev->ipc_config.periods_sink, sink_period_bytes);
835 ret = -ENOMEM;
836 goto err;
837 }
838
839 /* validate */
840 if (!sink_period_bytes) {
841 comp_err(dev, "src_prepare(): sink_period_bytes = 0");
842 ret = -EINVAL;
843 goto err;
844 }
845 if (!source_period_bytes) {
846 comp_err(dev, "src_prepare(): source_period_bytes = 0");
847 ret = -EINVAL;
848 goto err;
849 }
850
851 /* SRC supports S16_LE, S24_4LE and S32_LE formats */
852 if (source_format != sink_format) {
853 comp_err(dev, "src_prepare(): Source fmt %d and sink fmt %d are different.",
854 source_format, sink_format);
855 ret = -EINVAL;
856 goto err;
857 }
858
859 switch (source_format) {
860 #if CONFIG_FORMAT_S16LE
861 case SOF_IPC_FRAME_S16_LE:
862 cd->data_shift = 0;
863 cd->polyphase_func = src_polyphase_stage_cir_s16;
864 break;
865 #endif /* CONFIG_FORMAT_S16LE */
866 #if CONFIG_FORMAT_S24LE
867 case SOF_IPC_FRAME_S24_4LE:
868 cd->data_shift = 8;
869 cd->polyphase_func = src_polyphase_stage_cir;
870 break;
871 #endif /* CONFIG_FORMAT_S24LE */
872 #if CONFIG_FORMAT_S32LE
873 case SOF_IPC_FRAME_S32_LE:
874 cd->data_shift = 0;
875 cd->polyphase_func = src_polyphase_stage_cir;
876 break;
877 #endif /* CONFIG_FORMAT_S32LE */
878 default:
879 comp_err(dev, "src_prepare(): invalid format %d", source_format);
880 ret = -EINVAL;
881 goto err;
882 }
883
884 return 0;
885
886 err:
887 comp_set_state(dev, COMP_TRIGGER_RESET);
888 return ret;
889 }
890
src_reset(struct comp_dev * dev)891 static int src_reset(struct comp_dev *dev)
892 {
893 struct comp_data *cd = comp_get_drvdata(dev);
894
895 comp_info(dev, "src_reset()");
896
897 cd->src_func = src_fallback;
898 src_polyphase_reset(&cd->src);
899
900 comp_set_state(dev, COMP_TRIGGER_RESET);
901 return 0;
902 }
903
904 static const struct comp_driver comp_src = {
905 .type = SOF_COMP_SRC,
906 .uid = SOF_RT_UUID(src_uuid),
907 .tctx = &src_tr,
908 .ops = {
909 .create = src_new,
910 .free = src_free,
911 .params = src_params,
912 .cmd = src_cmd,
913 .trigger = src_trigger,
914 .copy = src_copy,
915 .prepare = src_prepare,
916 .reset = src_reset,
917 },
918 };
919
920 static SHARED_DATA struct comp_driver_info comp_src_info = {
921 .drv = &comp_src,
922 };
923
sys_comp_src_init(void)924 UT_STATIC void sys_comp_src_init(void)
925 {
926 comp_register(platform_shared_get(&comp_src_info,
927 sizeof(comp_src_info)));
928 }
929
930 DECLARE_MODULE(sys_comp_src_init);
931
