1 // SPDX-License-Identifier: BSD-3-Clause
2 //
3 // Copyright(c) 2019 Intel Corporation. All rights reserved.
4 //
5 // Author: Artur Kloniecki <arturx.kloniecki@linux.intel.com>
6
7 #if CONFIG_COMP_MUX
8
9 #include <sof/audio/buffer.h>
10 #include <sof/audio/component.h>
11 #include <sof/audio/format.h>
12 #include <sof/audio/mux.h>
13 #include <sof/bit.h>
14 #include <sof/common.h>
15 #include <ipc/stream.h>
16 #include <stddef.h>
17 #include <stdint.h>
18
mux_check_for_wrap(struct audio_stream * sink,const struct audio_stream ** sources,struct mux_look_up * lookup)19 static void mux_check_for_wrap(struct audio_stream *sink,
20 const struct audio_stream **sources,
21 struct mux_look_up *lookup)
22 {
23 const struct audio_stream *source;
24 uint32_t elem;
25
26 /* check sources and destinations for wrap */
27 for (elem = 0; elem < lookup->num_elems; elem++) {
28 source = sources[lookup->copy_elem[elem].stream_id];
29 lookup->copy_elem[elem].dest =
30 audio_stream_wrap(sink, lookup->copy_elem[elem].dest);
31 lookup->copy_elem[elem].src =
32 audio_stream_wrap(source, lookup->copy_elem[elem].src);
33 }
34 }
35
demux_check_for_wrap(struct audio_stream * sink,const struct audio_stream * source,struct mux_look_up * lookup)36 static void demux_check_for_wrap(struct audio_stream *sink,
37 const struct audio_stream *source,
38 struct mux_look_up *lookup)
39 {
40 uint32_t elem;
41
42 /* check sources and destinations for wrap */
43 for (elem = 0; elem < lookup->num_elems; elem++) {
44 lookup->copy_elem[elem].dest =
45 audio_stream_wrap(sink, lookup->copy_elem[elem].dest);
46 lookup->copy_elem[elem].src =
47 audio_stream_wrap(source, lookup->copy_elem[elem].src);
48 }
49 }
50
51 #if CONFIG_FORMAT_S16LE
52
demux_calc_frames_without_wrap_s16(struct comp_dev * dev,struct audio_stream * sink,const struct audio_stream * source,struct mux_look_up * lookup)53 static uint32_t demux_calc_frames_without_wrap_s16(struct comp_dev *dev,
54 struct audio_stream *sink,
55 const struct audio_stream
56 *source,
57 struct mux_look_up *lookup)
58 {
59 uint32_t frames;
60 uint32_t min_frames;
61 void *ptr;
62
63 /* for demux we process each source buffer separately - dest/src for
64 * each copy_elem refers to the same sink/source buffer, so min_frames
65 * calculation based only on lookup table first element is sufficient.
66 */
67 ptr = (int16_t *)lookup->copy_elem[0].dest -
68 lookup->copy_elem[0].out_ch;
69 min_frames = audio_stream_frames_without_wrap(sink, ptr);
70
71 ptr = (int16_t *)lookup->copy_elem[0].src -
72 lookup->copy_elem[0].in_ch;
73 frames = audio_stream_frames_without_wrap(source, ptr);
74
75 min_frames = (frames < min_frames) ? frames : min_frames;
76
77 return min_frames;
78 }
79
mux_calc_frames_without_wrap_s16(struct comp_dev * dev,struct audio_stream * sink,const struct audio_stream ** sources,struct mux_look_up * lookup)80 static uint32_t mux_calc_frames_without_wrap_s16(struct comp_dev *dev,
81 struct audio_stream *sink,
82 const struct audio_stream
83 **sources,
84 struct mux_look_up *lookup)
85 {
86 const struct audio_stream *source;
87 uint32_t frames;
88 uint32_t min_frames;
89 uint32_t elem;
90 void *ptr;
91
92 /* dest pointer for all copy_elems in lookup refers to the same
93 * sink buffer (mux has one sink buffer), so dest min_frames
94 * calculation based only on lookup table first element is sufficient.
95 */
96 ptr = (int16_t *)lookup->copy_elem[0].dest -
97 lookup->copy_elem[0].out_ch;
98 min_frames = audio_stream_frames_without_wrap(sink, ptr);
99
100 for (elem = 0; elem < lookup->num_elems; elem++) {
101 source = sources[lookup->copy_elem[elem].stream_id];
102
103 ptr = (int16_t *)lookup->copy_elem[elem].src -
104 lookup->copy_elem[elem].in_ch;
105 frames = audio_stream_frames_without_wrap(source, ptr);
106
107 min_frames = (frames < min_frames) ? frames : min_frames;
108 }
109
110 return min_frames;
111 }
112
mux_init_look_up_pointers_s16(struct comp_dev * dev,struct audio_stream * sink,const struct audio_stream ** sources,struct mux_look_up * lookup)113 static void mux_init_look_up_pointers_s16(struct comp_dev *dev,
114 struct audio_stream *sink,
115 const struct audio_stream **sources,
116 struct mux_look_up *lookup)
117 {
118 const struct audio_stream *source;
119 uint32_t elem;
120
121 /* init pointers */
122 for (elem = 0; elem < lookup->num_elems; elem++) {
123 source = sources[lookup->copy_elem[elem].stream_id];
124
125 lookup->copy_elem[elem].src = (int16_t *)source->r_ptr +
126 lookup->copy_elem[elem].in_ch;
127 lookup->copy_elem[elem].src_inc = source->channels;
128
129 lookup->copy_elem[elem].dest = (int16_t *)sink->w_ptr +
130 lookup->copy_elem[elem].out_ch;
131 lookup->copy_elem[elem].dest_inc = sink->channels;
132 }
133 }
134
demux_init_look_up_pointers_s16(struct comp_dev * dev,struct audio_stream * sink,const struct audio_stream * source,struct mux_look_up * lookup)135 static void demux_init_look_up_pointers_s16(struct comp_dev *dev,
136 struct audio_stream *sink,
137 const struct audio_stream *source,
138 struct mux_look_up *lookup)
139 {
140 uint32_t elem;
141
142 /* init pointers */
143 for (elem = 0; elem < lookup->num_elems; elem++) {
144 lookup->copy_elem[elem].src = (int16_t *)source->r_ptr +
145 lookup->copy_elem[elem].in_ch;
146 lookup->copy_elem[elem].src_inc = source->channels;
147
148 lookup->copy_elem[elem].dest = (int16_t *)sink->w_ptr +
149 lookup->copy_elem[elem].out_ch;
150 lookup->copy_elem[elem].dest_inc = sink->channels;
151 }
152 }
153
154 /**
155 * Source stream are routed to sinks with regard to look up table based on
156 * routing bitmasks from mux_stream_data structures array. Each sink channel
157 * has it's own lookup[].copy_elem describing source and sink fragment of
158 * memory featured in copying.
159 *
160 * @param[in] dev Component device
161 * @param[in,out] sink Destination buffer.
162 * @param[in,out] sources Array of source buffers.
163 * @param[in] frames Number of frames to process.
164 * @param[in] lookup mux look up table.
165 */
demux_s16le(struct comp_dev * dev,struct audio_stream * sink,const struct audio_stream * source,uint32_t frames,struct mux_look_up * lookup)166 static void demux_s16le(struct comp_dev *dev, struct audio_stream *sink,
167 const struct audio_stream *source, uint32_t frames,
168 struct mux_look_up *lookup)
169 {
170 uint8_t i;
171 int16_t *src;
172 int16_t *dst;
173 uint32_t elem;
174 uint32_t frames_without_wrap;
175
176 comp_dbg(dev, "demux_s16le()");
177
178 if (!lookup || !lookup->num_elems)
179 return;
180
181 demux_init_look_up_pointers_s16(dev, sink, source, lookup);
182
183 while (frames) {
184 frames_without_wrap =
185 demux_calc_frames_without_wrap_s16(dev, sink, source,
186 lookup);
187
188 frames_without_wrap = MIN(frames, frames_without_wrap);
189
190 for (i = 0; i < frames_without_wrap; i++) {
191 for (elem = 0; elem < lookup->num_elems; elem++) {
192 src = (int16_t *)lookup->copy_elem[elem].src;
193 dst = (int16_t *)lookup->copy_elem[elem].dest;
194 *dst = *src;
195 lookup->copy_elem[elem].src = src +
196 lookup->copy_elem[elem].src_inc;
197 lookup->copy_elem[elem].dest = dst +
198 lookup->copy_elem[elem].dest_inc;
199 }
200 }
201
202 demux_check_for_wrap(sink, source, lookup);
203
204 frames -= frames_without_wrap;
205 }
206 }
207
208 /**
209 * Source streams are routed to sink with regard to look up table based on
210 * routing bitmasks from mux_stream_data structures array. Each sink channel
211 * has it's own lookup[].copy_elem describing source and sink fragment of
212 * memory featured in copying.
213 *
214 * @param[in] dev Component device
215 * @param[in,out] sink Destination buffer.
216 * @param[in,out] sources Array of source buffers.
217 * @param[in] frames Number of frames to process.
218 * @param[in] lookup mux look up table.
219 */
mux_s16le(struct comp_dev * dev,struct audio_stream * sink,const struct audio_stream ** sources,uint32_t frames,struct mux_look_up * lookup)220 static void mux_s16le(struct comp_dev *dev, struct audio_stream *sink,
221 const struct audio_stream **sources, uint32_t frames,
222 struct mux_look_up *lookup)
223 {
224 uint8_t i;
225 int16_t *src;
226 int16_t *dst;
227 uint32_t elem;
228 uint32_t frames_without_wrap;
229
230 comp_dbg(dev, "mux_s16le()");
231
232 if (!lookup || !lookup->num_elems)
233 return;
234
235 mux_init_look_up_pointers_s16(dev, sink, sources, lookup);
236
237 while (frames) {
238 frames_without_wrap =
239 mux_calc_frames_without_wrap_s16(dev, sink, sources,
240 lookup);
241
242 frames_without_wrap = MIN(frames, frames_without_wrap);
243
244 for (i = 0; i < frames_without_wrap; i++) {
245 for (elem = 0; elem < lookup->num_elems; elem++) {
246 src = (int16_t *)lookup->copy_elem[elem].src;
247 dst = (int16_t *)lookup->copy_elem[elem].dest;
248 *dst = *src;
249 lookup->copy_elem[elem].src = src +
250 lookup->copy_elem[elem].src_inc;
251 lookup->copy_elem[elem].dest = dst +
252 lookup->copy_elem[elem].dest_inc;
253 }
254 }
255
256 mux_check_for_wrap(sink, sources, lookup);
257
258 frames -= frames_without_wrap;
259 }
260 }
261 #endif /* CONFIG_FORMAT_S16LE */
262
263 #if CONFIG_FORMAT_S24LE || CONFIG_FORMAT_S32LE
264
mux_calc_frames_without_wrap_s32(struct comp_dev * dev,struct audio_stream * sink,const struct audio_stream ** sources,struct mux_look_up * lookup)265 static uint32_t mux_calc_frames_without_wrap_s32(struct comp_dev *dev,
266 struct audio_stream *sink,
267 const struct audio_stream
268 **sources,
269 struct mux_look_up *lookup)
270 {
271 const struct audio_stream *source;
272 uint32_t frames;
273 uint32_t min_frames;
274 uint32_t elem;
275 void *ptr;
276
277 /* dest pointer for all copy_elems in lookup refers to the same
278 * sink buffer (mux has one sink buffer), so dest min_frames
279 * calculation based only on lookup table first element is sufficient.
280 */
281 ptr = (int32_t *)lookup->copy_elem[0].dest -
282 lookup->copy_elem[0].out_ch;
283 min_frames = audio_stream_frames_without_wrap(sink, ptr);
284
285 for (elem = 0; elem < lookup->num_elems; elem++) {
286 source = sources[lookup->copy_elem[elem].stream_id];
287
288 ptr = (int32_t *)lookup->copy_elem[elem].src -
289 lookup->copy_elem[elem].in_ch;
290 frames = audio_stream_frames_without_wrap(source, ptr);
291
292 min_frames = (frames < min_frames) ? frames : min_frames;
293 }
294
295 return min_frames;
296 }
297
demux_calc_frames_without_wrap_s32(struct comp_dev * dev,struct audio_stream * sink,const struct audio_stream * source,struct mux_look_up * lookup)298 static uint32_t demux_calc_frames_without_wrap_s32(struct comp_dev *dev,
299 struct audio_stream *sink,
300 const struct audio_stream
301 *source,
302 struct mux_look_up *lookup)
303 {
304 uint32_t frames;
305 uint32_t min_frames;
306 void *ptr;
307
308 /* for demux we process each source buffer separately - dest/src for
309 * each copy_elem refers to the same sink/source buffer, so min_frames
310 * calculation based only on lookup table first element is sufficient.
311 */
312 ptr = (int32_t *)lookup->copy_elem[0].dest -
313 lookup->copy_elem[0].out_ch;
314 min_frames = audio_stream_frames_without_wrap(sink, ptr);
315
316 ptr = (int32_t *)lookup->copy_elem[0].src -
317 lookup->copy_elem[0].in_ch;
318 frames = audio_stream_frames_without_wrap(source, ptr);
319
320 min_frames = (frames < min_frames) ? frames : min_frames;
321
322 return min_frames;
323 }
324
mux_init_look_up_pointers_s32(struct comp_dev * dev,struct audio_stream * sink,const struct audio_stream ** sources,struct mux_look_up * lookup)325 static void mux_init_look_up_pointers_s32(struct comp_dev *dev,
326 struct audio_stream *sink,
327 const struct audio_stream **sources,
328 struct mux_look_up *lookup)
329 {
330 const struct audio_stream *source;
331 uint32_t elem;
332
333 /* init pointers */
334 for (elem = 0; elem < lookup->num_elems; elem++) {
335 source = sources[lookup->copy_elem[elem].stream_id];
336
337 lookup->copy_elem[elem].src = (int32_t *)source->r_ptr +
338 lookup->copy_elem[elem].in_ch;
339 lookup->copy_elem[elem].src_inc = source->channels;
340
341 lookup->copy_elem[elem].dest = (int32_t *)sink->w_ptr +
342 lookup->copy_elem[elem].out_ch;
343 lookup->copy_elem[elem].dest_inc = sink->channels;
344 }
345 }
346
demux_init_look_up_pointers_s32(struct comp_dev * dev,struct audio_stream * sink,const struct audio_stream * source,struct mux_look_up * lookup)347 static void demux_init_look_up_pointers_s32(struct comp_dev *dev,
348 struct audio_stream *sink,
349 const struct audio_stream *source,
350 struct mux_look_up *lookup)
351 {
352 uint32_t elem;
353
354 /* init pointers */
355 for (elem = 0; elem < lookup->num_elems; elem++) {
356 lookup->copy_elem[elem].src = (int32_t *)source->r_ptr +
357 lookup->copy_elem[elem].in_ch;
358 lookup->copy_elem[elem].src_inc = source->channels;
359
360 lookup->copy_elem[elem].dest = (int32_t *)sink->w_ptr +
361 lookup->copy_elem[elem].out_ch;
362 lookup->copy_elem[elem].dest_inc = sink->channels;
363 }
364 }
365
366 /**
367 * Source stream are routed to sinks with regard to look up table based on
368 * routing bitmasks from mux_stream_data structures array. Each sink channel
369 * has it's own lookup[].copy_elem describing source and sink fragment of
370 * memory featured in copying.
371 *
372 * @param[in] dev Component device
373 * @param[in,out] sink Destination buffer.
374 * @param[in,out] sources Array of source buffers.
375 * @param[in] frames Number of frames to process.
376 * @param[in] lookup mux look up table.
377 */
demux_s32le(struct comp_dev * dev,struct audio_stream * sink,const struct audio_stream * source,uint32_t frames,struct mux_look_up * lookup)378 static void demux_s32le(struct comp_dev *dev, struct audio_stream *sink,
379 const struct audio_stream *source, uint32_t frames,
380 struct mux_look_up *lookup)
381 {
382 uint8_t i;
383 int32_t *src;
384 int32_t *dst;
385 uint32_t elem;
386 uint32_t frames_without_wrap;
387
388 comp_dbg(dev, "demux_s32le");
389
390 if (!lookup || !lookup->num_elems)
391 return;
392
393 demux_init_look_up_pointers_s32(dev, sink, source, lookup);
394
395 while (frames) {
396 frames_without_wrap =
397 demux_calc_frames_without_wrap_s32(dev, sink, source,
398 lookup);
399
400 frames_without_wrap = MIN(frames, frames_without_wrap);
401
402 for (i = 0; i < frames_without_wrap; i++) {
403 for (elem = 0; elem < lookup->num_elems; elem++) {
404 src = (int32_t *)lookup->copy_elem[elem].src;
405 dst = (int32_t *)lookup->copy_elem[elem].dest;
406 *dst = *src;
407 lookup->copy_elem[elem].src = src +
408 lookup->copy_elem[elem].src_inc;
409 lookup->copy_elem[elem].dest = dst +
410 lookup->copy_elem[elem].dest_inc;
411 }
412 }
413
414 demux_check_for_wrap(sink, source, lookup);
415
416 frames -= frames_without_wrap;
417 }
418 }
419
420 /**
421 * Source streams are routed to sink with regard to look up table based on
422 * routing bitmasks from mux_stream_data structures array. Each sink channel
423 * has it's own lookup[].copy_elem describing source and sink fragment of
424 * memory featured in copying.
425 *
426 * @param[in] dev Component device
427 * @param[in,out] sink Destination buffer.
428 * @param[in,out] sources Array of source buffers.
429 * @param[in] frames Number of frames to process.
430 * @param[in] lookup mux look up table.
431 */
mux_s32le(struct comp_dev * dev,struct audio_stream * sink,const struct audio_stream ** sources,uint32_t frames,struct mux_look_up * lookup)432 static void mux_s32le(struct comp_dev *dev, struct audio_stream *sink,
433 const struct audio_stream **sources, uint32_t frames,
434 struct mux_look_up *lookup)
435 {
436 uint8_t i;
437 int32_t *src;
438 int32_t *dst;
439 uint32_t elem;
440 uint32_t frames_without_wrap;
441
442 comp_dbg(dev, "mux_s32le()");
443
444 if (!lookup || !lookup->num_elems)
445 return;
446
447 mux_init_look_up_pointers_s32(dev, sink, sources, lookup);
448
449 while (frames) {
450 frames_without_wrap =
451 mux_calc_frames_without_wrap_s32(dev, sink, sources,
452 lookup);
453
454 frames_without_wrap = MIN(frames, frames_without_wrap);
455
456 for (i = 0; i < frames_without_wrap; i++) {
457 for (elem = 0; elem < lookup->num_elems; elem++) {
458 src = (int32_t *)lookup->copy_elem[elem].src;
459 dst = (int32_t *)lookup->copy_elem[elem].dest;
460 *dst = *src;
461 lookup->copy_elem[elem].src = src +
462 lookup->copy_elem[elem].src_inc;
463 lookup->copy_elem[elem].dest = dst +
464 lookup->copy_elem[elem].dest_inc;
465 }
466 }
467
468 mux_check_for_wrap(sink, sources, lookup);
469
470 frames -= frames_without_wrap;
471 }
472 }
473
474 #endif /* CONFIG_FORMAT_S24LE CONFIG_FORMAT_S32LE */
475
476 const struct comp_func_map mux_func_map[] = {
477 #if CONFIG_FORMAT_S16LE
478 { SOF_IPC_FRAME_S16_LE, &mux_s16le, &demux_s16le },
479 #endif
480 #if CONFIG_FORMAT_S24LE
481 { SOF_IPC_FRAME_S24_4LE, &mux_s32le, &demux_s32le },
482 #endif
483 #if CONFIG_FORMAT_S32LE
484 { SOF_IPC_FRAME_S32_LE, &mux_s32le, &demux_s32le },
485 #endif
486 };
487
mux_prepare_look_up_table(struct comp_dev * dev)488 void mux_prepare_look_up_table(struct comp_dev *dev)
489 {
490 struct comp_data *cd = comp_get_drvdata(dev);
491 uint8_t i;
492 uint8_t j;
493 uint8_t k;
494 uint8_t idx = 0;
495
496 /* Prepare look up table */
497 for (i = 0; i < cd->config.num_streams; i++) {
498 for (j = 0; j < PLATFORM_MAX_CHANNELS; j++) {
499 for (k = 0; k < PLATFORM_MAX_CHANNELS; k++) {
500 if (cd->config.streams[i].mask[j] & BIT(k)) {
501 /* MUX component has only one sink */
502 cd->lookup[0].copy_elem[idx].in_ch = j;
503 cd->lookup[0].copy_elem[idx].out_ch = k;
504 cd->lookup[0].copy_elem[idx].stream_id =
505 i;
506 cd->lookup[0].num_elems = ++idx;
507 }
508 }
509 }
510 }
511 }
512
demux_prepare_look_up_table(struct comp_dev * dev)513 void demux_prepare_look_up_table(struct comp_dev *dev)
514 {
515 struct comp_data *cd = comp_get_drvdata(dev);
516 uint8_t i;
517 uint8_t j;
518 uint8_t k;
519 uint8_t idx;
520
521 /* Prepare look up table */
522 for (i = 0; i < cd->config.num_streams; i++) {
523 idx = 0;
524 for (j = 0; j < PLATFORM_MAX_CHANNELS; j++) {
525 for (k = 0; k < PLATFORM_MAX_CHANNELS; k++) {
526 if (cd->config.streams[i].mask[j] & BIT(k)) {
527 /* DEMUX component has only one source */
528 cd->lookup[i].copy_elem[idx].in_ch = k;
529 cd->lookup[i].copy_elem[idx].out_ch = j;
530 cd->lookup[i].copy_elem[idx].stream_id =
531 i;
532 cd->lookup[i].num_elems = ++idx;
533 }
534 }
535 }
536 }
537 }
538
mux_get_processing_function(struct comp_dev * dev)539 mux_func mux_get_processing_function(struct comp_dev *dev)
540 {
541 struct comp_buffer *sinkb;
542 uint8_t i;
543
544 if (list_is_empty(&dev->bsink_list))
545 return NULL;
546
547 sinkb = list_first_item(&dev->bsink_list, struct comp_buffer,
548 source_list);
549
550 for (i = 0; i < ARRAY_SIZE(mux_func_map); i++) {
551 if (sinkb->stream.frame_fmt == mux_func_map[i].frame_format)
552 return mux_func_map[i].mux_proc_func;
553 }
554
555 return NULL;
556 }
557
demux_get_processing_function(struct comp_dev * dev)558 demux_func demux_get_processing_function(struct comp_dev *dev)
559 {
560 struct comp_buffer *sourceb;
561 uint8_t i;
562
563 if (list_is_empty(&dev->bsource_list))
564 return NULL;
565
566 sourceb = list_first_item(&dev->bsource_list, struct comp_buffer,
567 sink_list);
568
569 for (i = 0; i < ARRAY_SIZE(mux_func_map); i++) {
570 if (sourceb->stream.frame_fmt == mux_func_map[i].frame_format)
571 return mux_func_map[i].demux_proc_func;
572 }
573
574 return NULL;
575 }
576
577 #endif /* CONFIG_COMP_MUX */
578
