1 // SPDX-License-Identifier: BSD-3-Clause
2 //
3 // Copyright(c) 2022 Intel Corporation. All rights reserved.
4 //
5 // Author: Adrian Bonislawski <adrian.bonislawski@intel.com>
6
7 #include <sof/audio/aria/aria.h>
8 #include <sof/audio/buffer.h>
9 #include <sof/audio/format.h>
10 #include <sof/audio/pipeline.h>
11 #include <rtos/panic.h>
12 #include <sof/ipc/msg.h>
13 #include <rtos/alloc.h>
14 #include <rtos/cache.h>
15 #include <rtos/init.h>
16 #include <sof/lib/memory.h>
17 #include <sof/lib/notifier.h>
18 #include <sof/lib/uuid.h>
19 #include <sof/list.h>
20 #include <rtos/string.h>
21 #include <sof/ut.h>
22 #include <sof/trace/trace.h>
23 #include <ipc4/aria.h>
24 #include <ipc4/fw_reg.h>
25 #include <ipc/dai.h>
26 #include <user/trace.h>
27 #include <errno.h>
28 #include <stddef.h>
29 #include <stdint.h>
30
31 static const struct comp_driver comp_aria;
32
33 LOG_MODULE_REGISTER(aria, CONFIG_SOF_LOG_LEVEL);
34
35 /* these ids aligns windows driver requirement to support windows driver */
36 /* 99f7166d-372c-43ef-81f6-22007aa15f03 */
37 DECLARE_SOF_RT_UUID("aria", aria_comp_uuid, 0x99f7166d, 0x372c, 0x43ef,
38 0x81, 0xf6, 0x22, 0x00, 0x7a, 0xa1, 0x5f, 0x03);
39
40 DECLARE_TR_CTX(aria_comp_tr, SOF_UUID(aria_comp_uuid), LOG_LEVEL_INFO);
41
get_required_emory(size_t chan_cnt,size_t smpl_group_cnt)42 static size_t get_required_emory(size_t chan_cnt, size_t smpl_group_cnt)
43 {
44 /* Current implementation is able to apply 1 ms transition */
45 /* internal circular buffer aligned to 8 bytes */
46 const size_t num_of_ms = 1;
47
48 return ALIGN_UP(num_of_ms * chan_cnt * smpl_group_cnt, 2) * sizeof(int32_t);
49 }
50
aria_algo_init(struct comp_dev * dev,void * buffer_desc,void * buf_in,void * buf_out,size_t att,size_t chan_cnt,size_t smpl_group_cnt)51 static int aria_algo_init(struct comp_dev *dev, void *buffer_desc, void *buf_in,
52 void *buf_out, size_t att, size_t chan_cnt, size_t smpl_group_cnt)
53 {
54 struct aria_data *cd = comp_get_drvdata(dev);
55 size_t idx;
56
57 cd->chan_cnt = chan_cnt;
58 cd->smpl_group_cnt = smpl_group_cnt;
59 /* ensures buffer size is aligned to 8 bytes */
60 cd->buff_size = ALIGN_UP(cd->chan_cnt * cd->smpl_group_cnt, 2);
61 cd->offset = (cd->chan_cnt * cd->smpl_group_cnt) % 2;
62 cd->att = att;
63 cd->data = buffer_desc;
64 cd->buf_in = buf_in;
65 cd->buf_out = buf_out;
66 cd->buff_pos = 0;
67
68 for (idx = 0; idx < ARIA_MAX_GAIN_STATES; ++idx)
69 cd->gains[idx] = (1ULL << (32 - cd->att - 1)) - 1;
70
71 memset((void *)cd->data, 0, sizeof(int32_t) * cd->buff_size);
72 memset((void *)cd->buf_in, 0, sizeof(int32_t) * cd->buff_size);
73 memset((void *)cd->buf_out, 0, sizeof(int32_t) * cd->buff_size);
74 cd->gain_state = 0;
75
76 return 0;
77 }
78
aria_algo_buffer_data(struct comp_dev * dev,int32_t * __restrict data,size_t size)79 int aria_algo_buffer_data(struct comp_dev *dev, int32_t *__restrict data, size_t size)
80 {
81 struct aria_data *cd = comp_get_drvdata(dev);
82 size_t min_buff = MIN(cd->buff_size - cd->buff_pos - cd->offset, size);
83 int ret;
84
85 ret = memcpy_s(&cd->data[cd->buff_pos], cd->buff_size * sizeof(int32_t),
86 data, min_buff * sizeof(int32_t));
87 if (ret < 0)
88 return ret;
89 ret = memcpy_s(cd->data, cd->buff_size * sizeof(int32_t),
90 &data[min_buff], (size - min_buff) * sizeof(int32_t));
91 if (ret < 0)
92 return ret;
93 cd->buff_pos = (cd->buff_pos + size) % cd->buff_size;
94
95 return 0;
96 }
97
aria_process_data(struct comp_dev * dev,int32_t * __restrict dst,size_t dst_size,int32_t * __restrict src,size_t src_size)98 int aria_process_data(struct comp_dev *dev,
99 int32_t *__restrict dst, size_t dst_size,
100 int32_t *__restrict src, size_t src_size)
101 {
102 struct aria_data *cd = comp_get_drvdata(dev);
103 size_t min_buff;
104 int ret;
105
106 if (cd->att) {
107 aria_algo_calc_gain(dev, (cd->gain_state + 1) % ARIA_MAX_GAIN_STATES,
108 src, src_size);
109 aria_algo_get_data(dev, dst, dst_size);
110 } else {
111 /* bypass processing gets unprocessed data from buffer */
112 min_buff = MIN(cd->buff_size - cd->buff_pos - cd->offset, dst_size);
113 ret = memcpy_s(dst, cd->buff_size * sizeof(int32_t),
114 &cd->data[cd->buff_pos + cd->offset],
115 min_buff * sizeof(int32_t));
116 if (ret < 0)
117 return ret;
118 ret = memcpy_s(&dst[min_buff], cd->buff_size * sizeof(int32_t),
119 cd->data, (dst_size - min_buff) * sizeof(int32_t));
120 if (ret < 0)
121 return ret;
122 }
123 return aria_algo_buffer_data(dev, src, src_size);
124 }
125
init_aria(struct comp_dev * dev,const struct comp_ipc_config * config,const void * spec)126 static int init_aria(struct comp_dev *dev, const struct comp_ipc_config *config,
127 const void *spec)
128 {
129 const struct ipc4_aria_module_cfg *aria = spec;
130 struct aria_data *cd;
131 size_t ibs, chc, sgs, sgc, req_mem, att;
132 void *buf, *buf_in, *buf_out;
133 int ret;
134
135 dev->ipc_config = *config;
136 list_init(&dev->bsource_list);
137 list_init(&dev->bsink_list);
138
139 dcache_invalidate_region((__sparse_force void __sparse_cache *)spec, sizeof(*aria));
140 cd = rzalloc(SOF_MEM_ZONE_RUNTIME, 0, SOF_MEM_CAPS_RAM, sizeof(*cd));
141 if (!cd) {
142 comp_free(dev);
143 return -ENOMEM;
144 }
145
146 /* Copy received data format to local structures */
147 ret = memcpy_s(&cd->base, sizeof(struct ipc4_base_module_cfg),
148 &aria->base_cfg, sizeof(struct ipc4_base_module_cfg));
149 if (ret < 0)
150 return ret;
151
152 ibs = cd->base.ibs;
153 chc = cd->base.audio_fmt.channels_count;
154 sgs = (cd->base.audio_fmt.depth >> 3) * chc;
155 sgc = ibs / sgs;
156 req_mem = get_required_emory(chc, sgc);
157 att = aria->attenuation;
158
159 if (aria->attenuation > ARIA_MAX_ATT) {
160 comp_warn(dev, "init_aria(): Attenuation value %d must not be greater than %d",
161 att, ARIA_MAX_ATT);
162 att = ARIA_MAX_ATT;
163 }
164
165 comp_set_drvdata(dev, cd);
166
167 buf = rballoc(0, SOF_MEM_CAPS_RAM, req_mem);
168 buf_in = rballoc(0, SOF_MEM_CAPS_RAM, req_mem);
169 buf_out = rballoc(0, SOF_MEM_CAPS_RAM, req_mem);
170
171 if (!buf || !buf_in || !buf_out) {
172 rfree(buf);
173 rfree(buf_in);
174 rfree(buf_out);
175 rfree(cd);
176 comp_free(dev);
177 comp_err(dev, "init_aria(): allocation failed for size %d", req_mem);
178 return -ENOMEM;
179 }
180
181 return aria_algo_init(dev, buf, buf_in, buf_out, att, chc, sgc);
182 }
183
aria_new(const struct comp_driver * drv,const struct comp_ipc_config * config,const void * spec)184 static struct comp_dev *aria_new(const struct comp_driver *drv,
185 const struct comp_ipc_config *config,
186 const void *spec)
187 {
188 struct comp_dev *dev;
189 int ret;
190
191 comp_cl_info(&comp_aria, "aria_new()");
192
193 dev = comp_alloc(drv, sizeof(*dev));
194 if (!dev)
195 return NULL;
196
197 ret = init_aria(dev, config, spec);
198 if (ret < 0) {
199 comp_free(dev);
200 return NULL;
201 }
202 dev->state = COMP_STATE_READY;
203
204 return dev;
205 }
206
aria_free(struct comp_dev * dev)207 static void aria_free(struct comp_dev *dev)
208 {
209 struct aria_data *cd = comp_get_drvdata(dev);
210
211 rfree(cd->data);
212 rfree(cd->buf_in);
213 rfree(cd->buf_out);
214 rfree(cd);
215 rfree(dev);
216 }
217
aria_set_stream_params(struct comp_buffer * buffer,struct aria_data * cd)218 static void aria_set_stream_params(struct comp_buffer *buffer, struct aria_data *cd)
219 {
220 struct comp_buffer __sparse_cache *buffer_c;
221
222 buffer_c = buffer_acquire(buffer);
223 buffer_c->stream.channels = cd->chan_cnt;
224 buffer_c->stream.rate = cd->base.audio_fmt.sampling_frequency;
225 buffer_c->buffer_fmt = cd->base.audio_fmt.interleaving_style;
226 audio_stream_fmt_conversion(cd->base.audio_fmt.depth,
227 cd->base.audio_fmt.valid_bit_depth,
228 &buffer_c->stream.frame_fmt,
229 &buffer_c->stream.valid_sample_fmt,
230 cd->base.audio_fmt.s_type);
231 buffer_release(buffer_c);
232 }
233
aria_prepare(struct comp_dev * dev)234 static int aria_prepare(struct comp_dev *dev)
235 {
236 int ret;
237 struct comp_buffer *source, *sink;
238 struct aria_data *cd = comp_get_drvdata(dev);
239
240 comp_info(dev, "aria_prepare()");
241
242 source = list_first_item(&dev->bsource_list, struct comp_buffer, sink_list);
243 aria_set_stream_params(source, cd);
244
245 sink = list_first_item(&dev->bsink_list, struct comp_buffer, source_list);
246 aria_set_stream_params(sink, cd);
247
248 if (dev->state == COMP_STATE_ACTIVE) {
249 comp_info(dev, "aria_prepare(): Component is in active state.");
250 return 0;
251 }
252
253 ret = comp_set_state(dev, COMP_TRIGGER_PREPARE);
254 if (ret < 0)
255 return ret;
256
257 if (ret == COMP_STATUS_STATE_ALREADY_SET)
258 return PPL_STATUS_PATH_STOP;
259
260 return 0;
261 }
262
aria_reset(struct comp_dev * dev)263 static int aria_reset(struct comp_dev *dev)
264 {
265 struct aria_data *cd = comp_get_drvdata(dev);
266 int idx;
267
268 comp_info(dev, "aria_reset()");
269
270 if (dev->state == COMP_STATE_ACTIVE) {
271 comp_info(dev, "aria module is in active state. Ignore resetting");
272 return 0;
273 }
274
275 for (idx = 0; idx < ARIA_MAX_GAIN_STATES; ++idx)
276 cd->gains[idx] = (1ULL << (32 - cd->att - 1)) - 1;
277
278 memset(cd->data, 0, sizeof(int32_t) * cd->buff_size);
279 memset(cd->buf_in, 0, sizeof(int32_t) * cd->buff_size);
280 memset(cd->buf_out, 0, sizeof(int32_t) * cd->buff_size);
281 cd->gain_state = 0;
282
283 comp_set_state(dev, COMP_TRIGGER_RESET);
284
285 return 0;
286 }
287
aria_trigger(struct comp_dev * dev,int cmd)288 static int aria_trigger(struct comp_dev *dev, int cmd)
289 {
290 comp_info(dev, "aria_trigger()");
291
292 return comp_set_state(dev, cmd);
293 }
294
aria_copy(struct comp_dev * dev)295 static int aria_copy(struct comp_dev *dev)
296 {
297 struct comp_copy_limits c;
298 struct comp_buffer *source, *sink;
299 struct comp_buffer __sparse_cache *source_c, *sink_c;
300 struct aria_data *cd;
301 uint32_t source_bytes;
302 uint32_t sink_bytes;
303 uint32_t frag = 0;
304 int32_t *destp;
305 int32_t i, channel;
306
307 cd = comp_get_drvdata(dev);
308
309 comp_dbg(dev, "aria_copy()");
310
311 source = list_first_item(&dev->bsource_list, struct comp_buffer,
312 sink_list);
313 sink = list_first_item(&dev->bsink_list, struct comp_buffer,
314 source_list);
315
316 source_c = buffer_acquire(source);
317 sink_c = buffer_acquire(sink);
318
319 comp_get_copy_limits(source_c, sink_c, &c);
320 source_bytes = c.frames * c.source_frame_bytes;
321 sink_bytes = c.frames * c.sink_frame_bytes;
322
323 if (source_bytes == 0)
324 goto out;
325
326 buffer_stream_invalidate(source_c, source_bytes);
327
328 for (i = 0; i < c.frames; i++) {
329 for (channel = 0; channel < sink_c->stream.channels; channel++) {
330 cd->buf_in[frag] = *(int32_t *)audio_stream_read_frag_s32(&source_c->stream,
331 frag);
332
333 frag++;
334 }
335 }
336 dcache_writeback_region((__sparse_force void __sparse_cache *)cd->buf_in,
337 c.frames * sink_c->stream.channels * sizeof(int32_t));
338
339 aria_process_data(dev, cd->buf_out, sink_bytes / sizeof(uint32_t),
340 cd->buf_in, source_bytes / sizeof(uint32_t));
341
342 dcache_writeback_region((__sparse_force void __sparse_cache *)cd->buf_out,
343 c.frames * sink_c->stream.channels * sizeof(int32_t));
344 frag = 0;
345 for (i = 0; i < c.frames; i++) {
346 for (channel = 0; channel < sink_c->stream.channels; channel++) {
347 destp = audio_stream_write_frag_s32(&sink_c->stream, frag);
348 *destp = cd->buf_out[frag];
349
350 frag++;
351 }
352 }
353
354 buffer_stream_writeback(sink_c, sink_bytes);
355
356 comp_update_buffer_produce(sink_c, sink_bytes);
357 comp_update_buffer_consume(source_c, source_bytes);
358
359 out:
360 buffer_release(sink_c);
361 buffer_release(source_c);
362
363 return 0;
364 }
365
aria_get_attribute(struct comp_dev * dev,uint32_t type,void * value)366 static int aria_get_attribute(struct comp_dev *dev, uint32_t type, void *value)
367 {
368 struct aria_data *cd = comp_get_drvdata(dev);
369
370 switch (type) {
371 case COMP_ATTR_BASE_CONFIG:
372 *(struct ipc4_base_module_cfg *)value = cd->base;
373 break;
374 default:
375 return -EINVAL;
376 }
377
378 return 0;
379 }
380
381 static const struct comp_driver comp_aria = {
382 .uid = SOF_RT_UUID(aria_comp_uuid),
383 .tctx = &aria_comp_tr,
384 .ops = {
385 .create = aria_new,
386 .free = aria_free,
387 .trigger = aria_trigger,
388 .copy = aria_copy,
389 .prepare = aria_prepare,
390 .reset = aria_reset,
391 .get_attribute = aria_get_attribute,
392 },
393 };
394
395 static SHARED_DATA struct comp_driver_info comp_aria_info = {
396 .drv = &comp_aria,
397 };
398
sys_comp_aria_init(void)399 UT_STATIC void sys_comp_aria_init(void)
400 {
401 comp_register(platform_shared_get(&comp_aria_info,
402 sizeof(comp_aria_info)));
403 }
404
405 DECLARE_MODULE(sys_comp_aria_init);
406 SOF_MODULE_INIT(aria, sys_comp_aria_init);
407
