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/audio/component.h>
10 #include <sof/audio/buffer.h>
11 #include <sof/audio/eq_iir/eq_iir.h>
12 #include <sof/audio/eq_iir/iir.h>
13 #include <sof/audio/format.h>
14 #include <sof/audio/pipeline.h>
15 #include <sof/audio/ipc-config.h>
16 #include <sof/common.h>
17 #include <sof/debug/panic.h>
18 #include <sof/ipc/msg.h>
19 #include <sof/lib/alloc.h>
20 #include <sof/lib/memory.h>
21 #include <sof/lib/uuid.h>
22 #include <sof/list.h>
23 #include <sof/math/iir_df2t.h>
24 #include <sof/platform.h>
25 #include <sof/string.h>
26 #include <sof/ut.h>
27 #include <sof/trace/trace.h>
28 #include <ipc/control.h>
29 #include <ipc/stream.h>
30 #include <ipc/topology.h>
31 #include <user/eq.h>
32 #include <user/trace.h>
33 #include <errno.h>
34 #include <stddef.h>
35 #include <stdint.h>
36
37 static const struct comp_driver comp_eq_iir;
38
39 /* 5150c0e6-27f9-4ec8-8351-c705b642d12f */
40 DECLARE_SOF_RT_UUID("eq-iir", eq_iir_uuid, 0x5150c0e6, 0x27f9, 0x4ec8,
41 0x83, 0x51, 0xc7, 0x05, 0xb6, 0x42, 0xd1, 0x2f);
42
43 DECLARE_TR_CTX(eq_iir_tr, SOF_UUID(eq_iir_uuid), LOG_LEVEL_INFO);
44
45 /* IIR component private data */
46 struct comp_data {
47 struct iir_state_df2t iir[PLATFORM_MAX_CHANNELS]; /**< filters state */
48 struct comp_data_blob_handler *model_handler;
49 struct sof_eq_iir_config *config;
50 int64_t *iir_delay; /**< pointer to allocated RAM */
51 size_t iir_delay_size; /**< allocated size */
52 eq_iir_func eq_iir_func; /**< processing function */
53 };
54
55 #if CONFIG_FORMAT_S16LE
56 /*
57 * EQ IIR algorithm code
58 */
59
eq_iir_s16_default(const struct comp_dev * dev,const struct audio_stream * source,struct audio_stream * sink,uint32_t frames)60 static void eq_iir_s16_default(const struct comp_dev *dev,
61 const struct audio_stream *source,
62 struct audio_stream *sink,
63 uint32_t frames)
64
65 {
66 struct comp_data *cd = comp_get_drvdata(dev);
67 struct iir_state_df2t *filter;
68 int16_t *x;
69 int16_t *y;
70 int32_t z;
71 int ch;
72 int i;
73 int idx;
74 int nch = source->channels;
75
76 for (ch = 0; ch < nch; ch++) {
77 filter = &cd->iir[ch];
78 idx = ch;
79 for (i = 0; i < frames; i++) {
80 x = audio_stream_read_frag_s16(source, idx);
81 y = audio_stream_write_frag_s16(sink, idx);
82 z = iir_df2t(filter, *x << 16);
83 *y = sat_int16(Q_SHIFT_RND(z, 31, 15));
84 idx += nch;
85 }
86 }
87 }
88 #endif /* CONFIG_FORMAT_S16LE */
89
90 #if CONFIG_FORMAT_S24LE
eq_iir_s24_default(const struct comp_dev * dev,const struct audio_stream * source,struct audio_stream * sink,uint32_t frames)91 static void eq_iir_s24_default(const struct comp_dev *dev,
92 const struct audio_stream *source,
93 struct audio_stream *sink,
94 uint32_t frames)
95
96 {
97 struct comp_data *cd = comp_get_drvdata(dev);
98 struct iir_state_df2t *filter;
99 int32_t *x;
100 int32_t *y;
101 int32_t z;
102 int idx;
103 int ch;
104 int i;
105 int nch = source->channels;
106
107 for (ch = 0; ch < nch; ch++) {
108 filter = &cd->iir[ch];
109 idx = ch;
110 for (i = 0; i < frames; i++) {
111 x = audio_stream_read_frag_s32(source, idx);
112 y = audio_stream_write_frag_s32(sink, idx);
113 z = iir_df2t(filter, *x << 8);
114 *y = sat_int24(Q_SHIFT_RND(z, 31, 23));
115 idx += nch;
116 }
117 }
118 }
119 #endif /* CONFIG_FORMAT_S24LE */
120
121 #if CONFIG_FORMAT_S32LE
eq_iir_s32_default(const struct comp_dev * dev,const struct audio_stream * source,struct audio_stream * sink,uint32_t frames)122 static void eq_iir_s32_default(const struct comp_dev *dev,
123 const struct audio_stream *source,
124 struct audio_stream *sink,
125 uint32_t frames)
126
127 {
128 struct comp_data *cd = comp_get_drvdata(dev);
129 struct iir_state_df2t *filter;
130 int32_t *x;
131 int32_t *y;
132 int idx;
133 int ch;
134 int i;
135 int nch = source->channels;
136
137 for (ch = 0; ch < nch; ch++) {
138 filter = &cd->iir[ch];
139 idx = ch;
140 for (i = 0; i < frames; i++) {
141 x = audio_stream_read_frag_s32(source, idx);
142 y = audio_stream_write_frag_s32(sink, idx);
143 *y = iir_df2t(filter, *x);
144 idx += nch;
145 }
146 }
147 }
148 #endif /* CONFIG_FORMAT_S32LE */
149
150 #if CONFIG_FORMAT_S32LE && CONFIG_FORMAT_S16LE
eq_iir_s32_16_default(const struct comp_dev * dev,const struct audio_stream * source,struct audio_stream * sink,uint32_t frames)151 static void eq_iir_s32_16_default(const struct comp_dev *dev,
152 const struct audio_stream *source,
153 struct audio_stream *sink,
154 uint32_t frames)
155
156 {
157 struct comp_data *cd = comp_get_drvdata(dev);
158 struct iir_state_df2t *filter;
159 int32_t *x;
160 int16_t *y;
161 int32_t z;
162 int idx;
163 int ch;
164 int i;
165 int nch = source->channels;
166
167 for (ch = 0; ch < nch; ch++) {
168 filter = &cd->iir[ch];
169 idx = ch;
170 for (i = 0; i < frames; i++) {
171 x = audio_stream_read_frag_s32(source, idx);
172 y = audio_stream_write_frag_s16(sink, idx);
173 z = iir_df2t(filter, *x);
174 *y = sat_int16(Q_SHIFT_RND(z, 31, 15));
175 idx += nch;
176 }
177 }
178 }
179 #endif /* CONFIG_FORMAT_S32LE && CONFIG_FORMAT_S16LE */
180
181 #if CONFIG_FORMAT_S32LE && CONFIG_FORMAT_S24LE
eq_iir_s32_24_default(const struct comp_dev * dev,const struct audio_stream * source,struct audio_stream * sink,uint32_t frames)182 static void eq_iir_s32_24_default(const struct comp_dev *dev,
183 const struct audio_stream *source,
184 struct audio_stream *sink,
185 uint32_t frames)
186
187 {
188 struct comp_data *cd = comp_get_drvdata(dev);
189 struct iir_state_df2t *filter;
190 int32_t *x;
191 int32_t *y;
192 int32_t z;
193 int idx;
194 int ch;
195 int i;
196 int nch = source->channels;
197
198 for (ch = 0; ch < nch; ch++) {
199 filter = &cd->iir[ch];
200 idx = ch;
201 for (i = 0; i < frames; i++) {
202 x = audio_stream_read_frag_s32(source, idx);
203 y = audio_stream_write_frag_s32(sink, idx);
204 z = iir_df2t(filter, *x);
205 *y = sat_int24(Q_SHIFT_RND(z, 31, 23));
206 idx += nch;
207 }
208 }
209 }
210 #endif /* CONFIG_FORMAT_S32LE && CONFIG_FORMAT_S24LE */
211
eq_iir_pass(const struct comp_dev * dev,const struct audio_stream * source,struct audio_stream * sink,uint32_t frames)212 static void eq_iir_pass(const struct comp_dev *dev,
213 const struct audio_stream *source,
214 struct audio_stream *sink,
215 uint32_t frames)
216 {
217 audio_stream_copy(source, 0, sink, 0, frames * source->channels);
218 }
219
220 #if CONFIG_FORMAT_S16LE && CONFIG_FORMAT_S32LE
eq_iir_s32_s16_pass(const struct comp_dev * dev,const struct audio_stream * source,struct audio_stream * sink,uint32_t frames)221 static void eq_iir_s32_s16_pass(const struct comp_dev *dev,
222 const struct audio_stream *source,
223 struct audio_stream *sink,
224 uint32_t frames)
225 {
226 int32_t *x;
227 int16_t *y;
228 int i;
229 int n = frames * source->channels;
230
231 for (i = 0; i < n; i++) {
232 x = audio_stream_read_frag_s32(source, i);
233 y = audio_stream_write_frag_s16(sink, i);
234 *y = sat_int16(Q_SHIFT_RND(*x, 31, 15));
235 }
236 }
237 #endif /* CONFIG_FORMAT_S16LE && CONFIG_FORMAT_S32LE */
238
239 #if CONFIG_FORMAT_S24LE && CONFIG_FORMAT_S32LE
eq_iir_s32_s24_pass(const struct comp_dev * dev,const struct audio_stream * source,struct audio_stream * sink,uint32_t frames)240 static void eq_iir_s32_s24_pass(const struct comp_dev *dev,
241 const struct audio_stream *source,
242 struct audio_stream *sink,
243 uint32_t frames)
244 {
245 int32_t *x;
246 int32_t *y;
247 int i;
248 int n = frames * source->channels;
249
250 for (i = 0; i < n; i++) {
251 x = audio_stream_read_frag_s32(source, i);
252 y = audio_stream_write_frag_s16(sink, i);
253 *y = sat_int24(Q_SHIFT_RND(*x, 31, 23));
254 }
255 }
256 #endif /* CONFIG_FORMAT_S24LE && CONFIG_FORMAT_S32LE */
257
258 const struct eq_iir_func_map fm_configured[] = {
259 #if CONFIG_FORMAT_S16LE
260 {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S16_LE, eq_iir_s16_default},
261 #endif /* CONFIG_FORMAT_S16LE */
262 #if CONFIG_FORMAT_S16LE && CONFIG_FORMAT_S24LE
263 {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S24_4LE, NULL},
264 {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S16_LE, NULL},
265
266 #endif /* CONFIG_FORMAT_S16LE && CONFIG_FORMAT_S24LE */
267 #if CONFIG_FORMAT_S16LE && CONFIG_FORMAT_S32LE
268 {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S32_LE, NULL},
269 {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, eq_iir_s32_16_default},
270 #endif /* CONFIG_FORMAT_S16LE && CONFIG_FORMAT_S32LE */
271 #if CONFIG_FORMAT_S24LE
272 {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S24_4LE, eq_iir_s24_default},
273 #endif /* CONFIG_FORMAT_S24LE */
274 #if CONFIG_FORMAT_S24LE && CONFIG_FORMAT_S32LE
275 {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S32_LE, NULL},
276 {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, eq_iir_s32_24_default},
277 #endif /* CONFIG_FORMAT_S24LE && CONFIG_FORMAT_S32LE */
278 #if CONFIG_FORMAT_S32LE
279 {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, eq_iir_s32_default},
280 #endif /* CONFIG_FORMAT_S32LE */
281 };
282
283 const struct eq_iir_func_map fm_passthrough[] = {
284 #if CONFIG_FORMAT_S16LE
285 {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S16_LE, eq_iir_pass},
286 #endif /* CONFIG_FORMAT_S16LE */
287 #if CONFIG_FORMAT_S16LE && CONFIG_FORMAT_S24LE
288 {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S24_4LE, NULL},
289 {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S16_LE, NULL},
290
291 #endif /* CONFIG_FORMAT_S16LE && CONFIG_FORMAT_S24LE*/
292 #if CONFIG_FORMAT_S16LE && CONFIG_FORMAT_S32LE
293 {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S32_LE, NULL},
294 {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, eq_iir_s32_s16_pass},
295 #endif /* CONFIG_FORMAT_S16LE && CONFIG_FORMAT_S32LE*/
296 #if CONFIG_FORMAT_S24LE
297 {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S24_4LE, eq_iir_pass},
298 #endif /* CONFIG_FORMAT_S24LE */
299 #if CONFIG_FORMAT_S24LE && CONFIG_FORMAT_S32LE
300 {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S32_LE, NULL},
301 {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, eq_iir_s32_s24_pass},
302 #endif /* CONFIG_FORMAT_S24LE */
303 #if CONFIG_FORMAT_S32LE
304 {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, eq_iir_pass},
305 #endif /* CONFIG_FORMAT_S32LE */
306 };
307
eq_iir_find_func(enum sof_ipc_frame source_format,enum sof_ipc_frame sink_format,const struct eq_iir_func_map * map,int n)308 static eq_iir_func eq_iir_find_func(enum sof_ipc_frame source_format,
309 enum sof_ipc_frame sink_format,
310 const struct eq_iir_func_map *map,
311 int n)
312 {
313 int i;
314
315 /* Find suitable processing function from map. */
316 for (i = 0; i < n; i++) {
317 if ((uint8_t)source_format != map[i].source)
318 continue;
319 if ((uint8_t)sink_format != map[i].sink)
320 continue;
321
322 return map[i].func;
323 }
324
325 return NULL;
326 }
327
eq_iir_free_delaylines(struct comp_data * cd)328 static void eq_iir_free_delaylines(struct comp_data *cd)
329 {
330 struct iir_state_df2t *iir = cd->iir;
331 int i = 0;
332
333 /* Free the common buffer for all EQs and point then
334 * each IIR channel delay line to NULL.
335 */
336 rfree(cd->iir_delay);
337 cd->iir_delay = NULL;
338 cd->iir_delay_size = 0;
339 for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)
340 iir[i].delay = NULL;
341 }
342
eq_iir_init_coef(struct sof_eq_iir_config * config,struct iir_state_df2t * iir,int nch)343 static int eq_iir_init_coef(struct sof_eq_iir_config *config,
344 struct iir_state_df2t *iir, int nch)
345 {
346 struct sof_eq_iir_header_df2t *lookup[SOF_EQ_IIR_MAX_RESPONSES];
347 struct sof_eq_iir_header_df2t *eq;
348 int32_t *assign_response;
349 int32_t *coef_data;
350 int size_sum = 0;
351 int resp = 0;
352 int i;
353 int j;
354 int s;
355
356 comp_cl_info(&comp_eq_iir, "eq_iir_init_coef(), response assign for %u channels, %u responses",
357 config->channels_in_config,
358 config->number_of_responses);
359
360 /* Sanity checks */
361 if (nch > PLATFORM_MAX_CHANNELS ||
362 config->channels_in_config > PLATFORM_MAX_CHANNELS ||
363 !config->channels_in_config) {
364 comp_cl_err(&comp_eq_iir, "eq_iir_init_coef(), invalid channels count");
365 return -EINVAL;
366 }
367 if (config->number_of_responses > SOF_EQ_IIR_MAX_RESPONSES) {
368 comp_cl_err(&comp_eq_iir, "eq_iir_init_coef(), # of resp exceeds max");
369 return -EINVAL;
370 }
371
372 /* Collect index of response start positions in all_coefficients[] */
373 j = 0;
374 assign_response = ASSUME_ALIGNED(&config->data[0], 4);
375 coef_data = ASSUME_ALIGNED(&config->data[config->channels_in_config],
376 4);
377 for (i = 0; i < SOF_EQ_IIR_MAX_RESPONSES; i++) {
378 if (i < config->number_of_responses) {
379 eq = (struct sof_eq_iir_header_df2t *)&coef_data[j];
380 lookup[i] = eq;
381 j += SOF_EQ_IIR_NHEADER_DF2T
382 + SOF_EQ_IIR_NBIQUAD_DF2T * eq->num_sections;
383 } else {
384 lookup[i] = NULL;
385 }
386 }
387
388 /* Initialize 1st phase */
389 for (i = 0; i < nch; i++) {
390 /* Check for not reading past blob response to channel assign
391 * map. The previous channel response is assigned for any
392 * additional channels in the stream. It allows to use single
393 * channel configuration to setup multi channel equalization
394 * with the same response.
395 */
396 if (i < config->channels_in_config)
397 resp = assign_response[i];
398
399 if (resp < 0) {
400 /* Initialize EQ channel to bypass and continue with
401 * next channel response.
402 */
403 comp_cl_info(&comp_eq_iir, "eq_iir_init_coef(), ch %d is set to bypass",
404 i);
405 iir_reset_df2t(&iir[i]);
406 continue;
407 }
408
409 if (resp >= config->number_of_responses) {
410 comp_cl_info(&comp_eq_iir, "eq_iir_init_coef(), requested response %d exceeds defined",
411 resp);
412 return -EINVAL;
413 }
414
415 /* Initialize EQ coefficients */
416 eq = lookup[resp];
417 s = iir_delay_size_df2t(eq);
418 if (s > 0) {
419 size_sum += s;
420 } else {
421 comp_cl_info(&comp_eq_iir, "eq_iir_init_coef(), sections count %d exceeds max",
422 eq->num_sections);
423 return -EINVAL;
424 }
425
426 iir_init_coef_df2t(&iir[i], eq);
427 comp_cl_info(&comp_eq_iir, "eq_iir_init_coef(), ch %d is set to response %d",
428 i, resp);
429 }
430
431 return size_sum;
432 }
433
eq_iir_init_delay(struct iir_state_df2t * iir,int64_t * delay_start,int nch)434 static void eq_iir_init_delay(struct iir_state_df2t *iir,
435 int64_t *delay_start, int nch)
436 {
437 int64_t *delay = delay_start;
438 int i;
439
440 /* Initialize second phase to set EQ delay lines pointers. A
441 * bypass mode filter is indicated by biquads count of zero.
442 */
443 for (i = 0; i < nch; i++) {
444 if (iir[i].biquads > 0)
445 iir_init_delay_df2t(&iir[i], &delay);
446 }
447 }
448
eq_iir_setup(struct comp_data * cd,int nch)449 static int eq_iir_setup(struct comp_data *cd, int nch)
450 {
451 int delay_size;
452
453 /* Free existing IIR channels data if it was allocated */
454 eq_iir_free_delaylines(cd);
455
456 /* Set coefficients for each channel EQ from coefficient blob */
457 delay_size = eq_iir_init_coef(cd->config, cd->iir, nch);
458 if (delay_size < 0)
459 return delay_size; /* Contains error code */
460
461 /* If all channels were set to bypass there's no need to
462 * allocate delay. Just return with success.
463 */
464 if (!delay_size)
465 return 0;
466
467 /* Allocate all IIR channels data in a big chunk and clear it */
468 cd->iir_delay = rzalloc(SOF_MEM_ZONE_RUNTIME, 0, SOF_MEM_CAPS_RAM,
469 delay_size);
470 if (!cd->iir_delay) {
471 comp_cl_err(&comp_eq_iir, "eq_iir_setup(), delay allocation fail");
472 return -ENOMEM;
473 }
474
475 memset(cd->iir_delay, 0, delay_size);
476 cd->iir_delay_size = delay_size;
477
478 /* Assign delay line to each channel EQ */
479 eq_iir_init_delay(cd->iir, cd->iir_delay, nch);
480 return 0;
481 }
482
483 /*
484 * End of EQ setup code. Next the standard component methods.
485 */
486
eq_iir_new(const struct comp_driver * drv,struct comp_ipc_config * config,void * spec)487 static struct comp_dev *eq_iir_new(const struct comp_driver *drv,
488 struct comp_ipc_config *config,
489 void *spec)
490 {
491 struct comp_dev *dev = NULL;
492 struct comp_data *cd = NULL;
493 struct ipc_config_process *ipc_iir = spec;
494 size_t bs = ipc_iir->size;
495 int i;
496 int ret;
497
498 comp_cl_info(&comp_eq_iir, "eq_iir_new()");
499
500 /* Check first before proceeding with dev and cd that coefficients
501 * blob size is sane.
502 */
503 if (bs > SOF_EQ_IIR_MAX_SIZE) {
504 comp_cl_err(&comp_eq_iir, "eq_iir_new(), coefficients blob size %u exceeds maximum",
505 bs);
506 return NULL;
507 }
508
509 dev = comp_alloc(drv, sizeof(*dev));
510 if (!dev)
511 return NULL;
512 dev->ipc_config = *config;
513
514 cd = rzalloc(SOF_MEM_ZONE_RUNTIME, 0, SOF_MEM_CAPS_RAM, sizeof(*cd));
515 if (!cd)
516 goto fail;
517
518 comp_set_drvdata(dev, cd);
519
520 cd->eq_iir_func = NULL;
521 cd->iir_delay = NULL;
522 cd->iir_delay_size = 0;
523
524 /* component model data handler */
525 cd->model_handler = comp_data_blob_handler_new(dev);
526 if (!cd->model_handler) {
527 comp_cl_err(&comp_eq_iir, "eq_iir_new(): comp_data_blob_handler_new() failed.");
528 goto cd_fail;
529 }
530
531 /* Allocate and make a copy of the coefficients blob and reset IIR. If
532 * the EQ is configured later in run-time the size is zero.
533 */
534 ret = comp_init_data_blob(cd->model_handler, bs, ipc_iir->data);
535 if (ret < 0) {
536 comp_cl_err(&comp_eq_iir, "eq_iir_new(): comp_init_data_blob() failed.");
537 goto cd_fail;
538 }
539
540 for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)
541 iir_reset_df2t(&cd->iir[i]);
542
543 dev->state = COMP_STATE_READY;
544 return dev;
545
546 cd_fail:
547 comp_data_blob_handler_free(cd->model_handler);
548 rfree(cd);
549 fail:
550 rfree(dev);
551 return NULL;
552 }
553
eq_iir_free(struct comp_dev * dev)554 static void eq_iir_free(struct comp_dev *dev)
555 {
556 struct comp_data *cd = comp_get_drvdata(dev);
557
558 comp_info(dev, "eq_iir_free()");
559
560 eq_iir_free_delaylines(cd);
561 comp_data_blob_handler_free(cd->model_handler);
562
563 rfree(cd);
564 rfree(dev);
565 }
566
eq_iir_verify_params(struct comp_dev * dev,struct sof_ipc_stream_params * params)567 static int eq_iir_verify_params(struct comp_dev *dev,
568 struct sof_ipc_stream_params *params)
569 {
570 struct comp_buffer *sourceb;
571 struct comp_buffer *sinkb;
572 uint32_t buffer_flag;
573 int ret;
574
575 comp_dbg(dev, "eq_iir_verify_params()");
576
577 /* EQ component will only ever have 1 source and 1 sink buffer */
578 sourceb = list_first_item(&dev->bsource_list, struct comp_buffer,
579 sink_list);
580 sinkb = list_first_item(&dev->bsink_list, struct comp_buffer,
581 source_list);
582
583 /* we check whether we can support frame_fmt conversion (whether we have
584 * such conversion function) due to source and sink buffer frame_fmt's.
585 * If not, we will overwrite sink (playback) and source (capture) with
586 * pcm frame_fmt and will not make any conversion (sink and source
587 * frame_fmt will be equal).
588 */
589 buffer_flag = eq_iir_find_func(sourceb->stream.frame_fmt,
590 sinkb->stream.frame_fmt, fm_configured,
591 ARRAY_SIZE(fm_configured)) ?
592 BUFF_PARAMS_FRAME_FMT : 0;
593
594 ret = comp_verify_params(dev, buffer_flag, params);
595 if (ret < 0) {
596 comp_err(dev, "eq_iir_verify_params(): comp_verify_params() failed.");
597 return ret;
598 }
599
600 return 0;
601 }
602
603 /* set component audio stream parameters */
eq_iir_params(struct comp_dev * dev,struct sof_ipc_stream_params * params)604 static int eq_iir_params(struct comp_dev *dev,
605 struct sof_ipc_stream_params *params)
606 {
607 int err;
608
609 comp_info(dev, "eq_iir_params()");
610
611 err = eq_iir_verify_params(dev, params);
612 if (err < 0) {
613 comp_err(dev, "eq_iir_params(): pcm params verification failed.");
614 return -EINVAL;
615 }
616
617 /* All configuration work is postponed to prepare(). */
618 return 0;
619 }
620
iir_cmd_get_data(struct comp_dev * dev,struct sof_ipc_ctrl_data * cdata,int max_size)621 static int iir_cmd_get_data(struct comp_dev *dev,
622 struct sof_ipc_ctrl_data *cdata, int max_size)
623 {
624 struct comp_data *cd = comp_get_drvdata(dev);
625 int ret = 0;
626
627 switch (cdata->cmd) {
628 case SOF_CTRL_CMD_BINARY:
629 comp_info(dev, "iir_cmd_get_data(), SOF_CTRL_CMD_BINARY");
630 ret = comp_data_blob_get_cmd(cd->model_handler, cdata,
631 max_size);
632 break;
633 default:
634 comp_err(dev, "iir_cmd_get_data(), invalid command");
635 ret = -EINVAL;
636 break;
637 }
638 return ret;
639 }
640
iir_cmd_set_data(struct comp_dev * dev,struct sof_ipc_ctrl_data * cdata)641 static int iir_cmd_set_data(struct comp_dev *dev,
642 struct sof_ipc_ctrl_data *cdata)
643 {
644 struct comp_data *cd = comp_get_drvdata(dev);
645 int ret = 0;
646
647 switch (cdata->cmd) {
648 case SOF_CTRL_CMD_BINARY:
649 comp_info(dev, "iir_cmd_set_data(), SOF_CTRL_CMD_BINARY");
650 ret = comp_data_blob_set_cmd(cd->model_handler, cdata);
651 break;
652 default:
653 comp_err(dev, "iir_cmd_set_data(), invalid command");
654 ret = -EINVAL;
655 break;
656 }
657
658 return ret;
659 }
660
661 /* used to pass standard and bespoke commands (with data) to component */
eq_iir_cmd(struct comp_dev * dev,int cmd,void * data,int max_data_size)662 static int eq_iir_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, "eq_iir_cmd()");
669
670 switch (cmd) {
671 case COMP_CMD_SET_DATA:
672 ret = iir_cmd_set_data(dev, cdata);
673 break;
674 case COMP_CMD_GET_DATA:
675 ret = iir_cmd_get_data(dev, cdata, max_data_size);
676 break;
677 default:
678 comp_err(dev, "eq_iir_cmd(), invalid command");
679 ret = -EINVAL;
680 }
681
682 return ret;
683 }
684
eq_iir_trigger(struct comp_dev * dev,int cmd)685 static int eq_iir_trigger(struct comp_dev *dev, int cmd)
686 {
687 struct comp_data *cd = comp_get_drvdata(dev);
688
689 comp_info(dev, "eq_iir_trigger()");
690
691 if (cmd == COMP_TRIGGER_START || cmd == COMP_TRIGGER_RELEASE)
692 assert(cd->eq_iir_func);
693
694 return comp_set_state(dev, cmd);
695 }
696
eq_iir_process(struct comp_dev * dev,struct comp_buffer * source,struct comp_buffer * sink,int frames,uint32_t source_bytes,uint32_t sink_bytes)697 static void eq_iir_process(struct comp_dev *dev, struct comp_buffer *source,
698 struct comp_buffer *sink, int frames,
699 uint32_t source_bytes, uint32_t sink_bytes)
700 {
701 struct comp_data *cd = comp_get_drvdata(dev);
702
703 buffer_invalidate(source, source_bytes);
704
705 cd->eq_iir_func(dev, &source->stream, &sink->stream, frames);
706
707 buffer_writeback(sink, sink_bytes);
708
709 /* calc new free and available */
710 comp_update_buffer_consume(source, source_bytes);
711 comp_update_buffer_produce(sink, sink_bytes);
712 }
713
714 /* copy and process stream data from source to sink buffers */
eq_iir_copy(struct comp_dev * dev)715 static int eq_iir_copy(struct comp_dev *dev)
716 {
717 struct comp_copy_limits cl;
718 struct comp_data *cd = comp_get_drvdata(dev);
719 struct comp_buffer *sourceb;
720 struct comp_buffer *sinkb;
721 int ret;
722
723 comp_dbg(dev, "eq_iir_copy()");
724
725 sourceb = list_first_item(&dev->bsource_list, struct comp_buffer,
726 sink_list);
727
728 /* Check for changed configuration */
729 if (comp_is_new_data_blob_available(cd->model_handler)) {
730 cd->config = comp_get_data_blob(cd->model_handler, NULL, NULL);
731 ret = eq_iir_setup(cd, sourceb->stream.channels);
732 if (ret < 0) {
733 comp_err(dev, "eq_iir_copy(), failed IIR setup");
734 return ret;
735 }
736 }
737
738 sinkb = list_first_item(&dev->bsink_list, struct comp_buffer,
739 source_list);
740
741 /* Get source, sink, number of frames etc. to process. */
742 comp_get_copy_limits_with_lock(sourceb, sinkb, &cl);
743
744 /* Run EQ function */
745 eq_iir_process(dev, sourceb, sinkb, cl.frames, cl.source_bytes,
746 cl.sink_bytes);
747
748 return 0;
749 }
750
eq_iir_prepare(struct comp_dev * dev)751 static int eq_iir_prepare(struct comp_dev *dev)
752 {
753 struct comp_data *cd = comp_get_drvdata(dev);
754 struct comp_buffer *sourceb;
755 struct comp_buffer *sinkb;
756 enum sof_ipc_frame source_format;
757 enum sof_ipc_frame sink_format;
758 uint32_t sink_period_bytes;
759 int ret;
760
761 comp_info(dev, "eq_iir_prepare()");
762
763 ret = comp_set_state(dev, COMP_TRIGGER_PREPARE);
764 if (ret < 0)
765 return ret;
766
767 if (ret == COMP_STATUS_STATE_ALREADY_SET)
768 return PPL_STATUS_PATH_STOP;
769
770 /* EQ component will only ever have 1 source and 1 sink buffer */
771 sourceb = list_first_item(&dev->bsource_list,
772 struct comp_buffer, sink_list);
773 sinkb = list_first_item(&dev->bsink_list,
774 struct comp_buffer, source_list);
775
776 /* get source data format */
777 source_format = sourceb->stream.frame_fmt;
778
779 /* get sink data format and period bytes */
780 sink_format = sinkb->stream.frame_fmt;
781 sink_period_bytes = audio_stream_period_bytes(&sinkb->stream,
782 dev->frames);
783
784 if (sinkb->stream.size < sink_period_bytes) {
785 comp_err(dev, "eq_iir_prepare(): sink buffer size %d is insufficient < %d",
786 sinkb->stream.size, sink_period_bytes);
787 ret = -ENOMEM;
788 goto err;
789 }
790
791 cd->config = comp_get_data_blob(cd->model_handler, NULL, NULL);
792
793 /* Initialize EQ */
794 comp_info(dev, "eq_iir_prepare(), source_format=%d, sink_format=%d",
795 source_format, sink_format);
796 if (cd->config) {
797 ret = eq_iir_setup(cd, sourceb->stream.channels);
798 if (ret < 0) {
799 comp_err(dev, "eq_iir_prepare(), setup failed.");
800 goto err;
801 }
802 cd->eq_iir_func = eq_iir_find_func(source_format, sink_format, fm_configured,
803 ARRAY_SIZE(fm_configured));
804 if (!cd->eq_iir_func) {
805 comp_err(dev, "eq_iir_prepare(), No proc func");
806 ret = -EINVAL;
807 goto err;
808 }
809 comp_info(dev, "eq_iir_prepare(), IIR is configured.");
810 } else {
811 cd->eq_iir_func = eq_iir_find_func(source_format, sink_format, fm_passthrough,
812 ARRAY_SIZE(fm_passthrough));
813 if (!cd->eq_iir_func) {
814 comp_err(dev, "eq_iir_prepare(), No pass func");
815 ret = -EINVAL;
816 goto err;
817 }
818 comp_info(dev, "eq_iir_prepare(), pass-through mode.");
819 }
820 return 0;
821
822 err:
823 comp_set_state(dev, COMP_TRIGGER_RESET);
824 return ret;
825 }
826
eq_iir_reset(struct comp_dev * dev)827 static int eq_iir_reset(struct comp_dev *dev)
828 {
829 int i;
830 struct comp_data *cd = comp_get_drvdata(dev);
831
832 comp_info(dev, "eq_iir_reset()");
833
834 eq_iir_free_delaylines(cd);
835
836 cd->eq_iir_func = NULL;
837 for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)
838 iir_reset_df2t(&cd->iir[i]);
839
840 comp_set_state(dev, COMP_TRIGGER_RESET);
841 return 0;
842 }
843
844 static const struct comp_driver comp_eq_iir = {
845 .type = SOF_COMP_EQ_IIR,
846 .uid = SOF_RT_UUID(eq_iir_uuid),
847 .tctx = &eq_iir_tr,
848 .ops = {
849 .create = eq_iir_new,
850 .free = eq_iir_free,
851 .params = eq_iir_params,
852 .cmd = eq_iir_cmd,
853 .trigger = eq_iir_trigger,
854 .copy = eq_iir_copy,
855 .prepare = eq_iir_prepare,
856 .reset = eq_iir_reset,
857 },
858 };
859
860 static SHARED_DATA struct comp_driver_info comp_eq_iir_info = {
861 .drv = &comp_eq_iir,
862 };
863
sys_comp_eq_iir_init(void)864 UT_STATIC void sys_comp_eq_iir_init(void)
865 {
866 comp_register(platform_shared_get(&comp_eq_iir_info,
867 sizeof(comp_eq_iir_info)));
868 }
869
870 DECLARE_MODULE(sys_comp_eq_iir_init);
871
