1 // SPDX-License-Identifier: BSD-3-Clause
2 //
3 // Copyright(c) 2019-2022 Intel Corporation. All rights reserved.
4 //
5 // Author: Artur Kloniecki <arturx.kloniecki@linux.intel.com>
6 // Author: Adrian Bonislawski <adrian.bonislawski@linux.intel.com>
7
8 #include <sof/audio/buffer.h>
9 #include <sof/audio/component.h>
10 #include <sof/probe/probe.h>
11 #include <sof/trace/trace.h>
12 #include <user/trace.h>
13 #include <rtos/alloc.h>
14 #include <rtos/init.h>
15 #include <sof/lib/dma.h>
16 #include <sof/lib/notifier.h>
17 #include <sof/lib/uuid.h>
18 #include <sof/ipc/topology.h>
19 #include <sof/ipc/driver.h>
20 #include <rtos/timer.h>
21 #include <sof/schedule/ll_schedule.h>
22 #include <sof/schedule/schedule.h>
23 #include <rtos/task.h>
24 #if CONFIG_IPC_MAJOR_4
25 #include <ipc4/gateway.h>
26 #include <ipc4/module.h>
27 #include <sof/audio/component.h>
28 #include <sof/audio/component_ext.h>
29 #include <sof/ut.h>
30
31 /* 7CAD0808-AB10-CD23-EF45-12AB34CD56EF */
32 DECLARE_SOF_RT_UUID("probe", probe_uuid, 0x7CAD0808, 0xAB10, 0xCD23,
33 0xEF, 0x45, 0x12, 0xAB, 0x34, 0xCD, 0x56, 0xEF);
34
35 static const struct comp_driver comp_probe;
36 #elif CONFIG_IPC_MAJOR_3
37 /* 9d1fb66e-4ffb-497f-994b-17719686596e */
38 DECLARE_SOF_RT_UUID("probe", probe_uuid, 0x9d1fb66e, 0x4ffb, 0x497f,
39 0x99, 0x4b, 0x17, 0x71, 0x96, 0x86, 0x59, 0x6e);
40 #else
41 #error "No or invalid IPC MAJOR version selected."
42 #endif /* CONFIG_IPC_MAJOR_4 */
43
44 DECLARE_TR_CTX(pr_tr, SOF_UUID(probe_uuid), LOG_LEVEL_INFO);
45
46 /* 2f0b1901-cac0-4b87-812f-f2d5e4f19e4a */
47 DECLARE_SOF_UUID("probe-task", probe_task_uuid, 0x2f0b1901, 0xcac0, 0x4b87,
48 0x81, 0x2f, 0xf2, 0xd5, 0xe4, 0xf1, 0x9e, 0x4a);
49
50 LOG_MODULE_REGISTER(probe, CONFIG_SOF_LOG_LEVEL);
51
52 #define PROBE_DMA_INVALID 0xFFFFFFFF
53 #define PROBE_POINT_INVALID 0xFFFFFFFF
54
55 #define PROBE_BUFFER_LOCAL_SIZE 8192
56 #define DMA_ELEM_SIZE 32
57
58 /**
59 * DMA buffer
60 */
61 struct probe_dma_buf {
62 uintptr_t w_ptr; /**< write pointer */
63 uintptr_t r_ptr; /**< read pointer */
64 uintptr_t addr; /**< buffer start pointer */
65 uintptr_t end_addr; /**< buffer end pointer */
66 uint32_t size; /**< buffer size */
67 uint32_t avail; /**< buffer avail data */
68 };
69
70 /**
71 * Probe DMA
72 */
73 struct probe_dma_ext {
74 uint32_t stream_tag; /**< DMA stream tag */
75 uint32_t dma_buffer_size; /**< DMA buffer size */
76 struct dma_sg_config config; /**< DMA SG config */
77 struct probe_dma_buf dmapb; /**< DMA buffer pointer */
78 struct dma_copy dc; /**< DMA copy */
79 };
80
81 /**
82 * Probe main struct
83 */
84 struct probe_pdata {
85 struct probe_dma_ext ext_dma; /**< extraction DMA */
86 struct probe_dma_ext inject_dma[CONFIG_PROBE_DMA_MAX]; /**< injection DMA */
87 struct probe_point probe_points[CONFIG_PROBE_POINTS_MAX]; /**< probe points */
88 struct probe_data_packet header; /**< data packet header */
89 struct task dmap_work; /**< probe task */
90 };
91
92 /**
93 * \brief Allocate and initialize probe buffer with correct alignment.
94 * \param[out] buffer return value.
95 * \param[in] size of buffer.
96 * \param[in] align the buffer.
97 * \return 0 on success, error code otherwise.
98 */
probe_dma_buffer_init(struct probe_dma_buf * buffer,uint32_t size,uint32_t align)99 static int probe_dma_buffer_init(struct probe_dma_buf *buffer, uint32_t size,
100 uint32_t align)
101 {
102 /* allocate new buffer */
103 buffer->addr = (uintptr_t)rballoc_align(0, SOF_MEM_CAPS_DMA,
104 size, align);
105
106 if (!buffer->addr) {
107 tr_err(&pr_tr, "probe_dma_buffer_init(): alloc failed");
108 return -ENOMEM;
109 }
110
111 bzero((void *)buffer->addr, size);
112 dcache_writeback_region((__sparse_force void __sparse_cache *)buffer->addr, size);
113
114 /* initialise the DMA buffer */
115 buffer->size = size;
116 buffer->w_ptr = buffer->addr;
117 buffer->r_ptr = buffer->addr;
118 buffer->end_addr = buffer->addr + buffer->size;
119 buffer->avail = 0;
120
121 return 0;
122 }
123
124 /**
125 * \brief Request DMA and initialize DMA for probes with correct alignment,
126 * size and specific channel.
127 *
128 * \param[out] dma probe returned
129 * \param[in] direction of the DMA
130 * \return 0 on success, error code otherwise.
131 */
probe_dma_init(struct probe_dma_ext * dma,uint32_t direction)132 static int probe_dma_init(struct probe_dma_ext *dma, uint32_t direction)
133 {
134 struct dma_sg_config config = { 0 };
135 uint32_t elem_addr, addr_align;
136 const uint32_t elem_size = sizeof(uint64_t) * DMA_ELEM_SIZE;
137 const uint32_t elem_num = PROBE_BUFFER_LOCAL_SIZE / elem_size;
138 uint32_t channel;
139 int err = 0;
140
141 #if CONFIG_IPC_MAJOR_4
142 channel = ((union ipc4_connector_node_id)dma->stream_tag).f.v_index + 1;
143 #else
144 channel = dma->stream_tag;
145 #endif
146 /* request DMA in the dir LMEM->HMEM with shared access */
147 dma->dc.dmac = dma_get(direction, 0, DMA_DEV_HOST,
148 DMA_ACCESS_SHARED);
149 if (!dma->dc.dmac) {
150 tr_err(&pr_tr, "probe_dma_init(): dma->dc.dmac = NULL");
151 return -ENODEV;
152 }
153
154 /* get required address alignment for dma buffer */
155 #if CONFIG_ZEPHYR_NATIVE_DRIVERS
156 err = dma_get_attribute(dma->dc.dmac->z_dev, DMA_ATTR_BUFFER_ADDRESS_ALIGNMENT,
157 &addr_align);
158 #else
159 err = dma_get_attribute_legacy(dma->dc.dmac, DMA_ATTR_BUFFER_ADDRESS_ALIGNMENT,
160 &addr_align);
161 #endif
162 if (err < 0)
163 return err;
164
165 /* initialize dma buffer */
166 err = probe_dma_buffer_init(&dma->dmapb, PROBE_BUFFER_LOCAL_SIZE, addr_align);
167 if (err < 0)
168 return err;
169
170 err = dma_copy_set_stream_tag(&dma->dc, channel);
171 if (err < 0)
172 return err;
173
174 elem_addr = (uint32_t)dma->dmapb.addr;
175
176 config.direction = direction;
177 config.src_width = sizeof(uint32_t);
178 config.dest_width = sizeof(uint32_t);
179 config.cyclic = 0;
180
181 err = dma_sg_alloc(&config.elem_array, SOF_MEM_ZONE_RUNTIME,
182 config.direction, elem_num, elem_size, elem_addr, 0);
183 if (err < 0)
184 return err;
185
186 err = dma_set_config_legacy(dma->dc.chan, &config);
187 if (err < 0)
188 return err;
189
190 dma_sg_free(&config.elem_array);
191
192 return 0;
193 }
194
195 /**
196 * \brief Stop, deinit and free DMA and buffer used by probes.
197 *
198 * \return 0 on success, error code otherwise.
199 */
probe_dma_deinit(struct probe_dma_ext * dma)200 static int probe_dma_deinit(struct probe_dma_ext *dma)
201 {
202 int err = 0;
203
204 err = dma_stop_legacy(dma->dc.chan);
205 if (err < 0) {
206 tr_err(&pr_tr, "probe_dma_deinit(): dma_stop() failed");
207 return err;
208 }
209
210 dma_channel_put_legacy(dma->dc.chan);
211 dma_put(dma->dc.dmac);
212
213 rfree((void *)dma->dmapb.addr);
214 dma->dmapb.addr = 0;
215
216 dma->stream_tag = PROBE_DMA_INVALID;
217
218 return 0;
219 }
220
221 /*
222 * \brief Probe task for extraction.
223 *
224 * Copy extraction probes data to host if available.
225 * Return err if dma copy failed.
226 */
probe_task(void * data)227 static enum task_state probe_task(void *data)
228 {
229 struct probe_pdata *_probe = probe_get();
230 uint32_t copy_align, avail;
231 int err;
232
233 if (!_probe->ext_dma.dmapb.avail)
234 return SOF_TASK_STATE_RESCHEDULE;
235 #if CONFIG_ZEPHYR_NATIVE_DRIVERS
236 err = dma_get_attribute(_probe->ext_dma.dc.dmac->z_dev, DMA_ATTR_COPY_ALIGNMENT,
237 ©_align);
238 #else
239 err = dma_get_attribute_legacy(_probe->ext_dma.dc.dmac, DMA_ATTR_COPY_ALIGNMENT,
240 ©_align);
241 #endif
242 if (err < 0) {
243 tr_err(&pr_tr, "probe_task(): dma_get_attribute failed.");
244 return SOF_TASK_STATE_COMPLETED;
245 }
246
247 avail = ALIGN_DOWN(_probe->ext_dma.dmapb.avail, copy_align);
248 if (avail + _probe->ext_dma.dmapb.r_ptr >= _probe->ext_dma.dmapb.end_addr)
249 avail = _probe->ext_dma.dmapb.end_addr - _probe->ext_dma.dmapb.r_ptr;
250
251 if (avail > 0)
252 err = dma_copy_to_host_nowait(&_probe->ext_dma.dc,
253 &_probe->ext_dma.config, 0,
254 (void *)_probe->ext_dma.dmapb.r_ptr,
255 avail);
256 else
257 return SOF_TASK_STATE_RESCHEDULE;
258
259 if (err < 0) {
260 tr_err(&pr_tr, "probe_task(): dma_copy_to_host() failed.");
261 return err;
262 }
263
264 /* buffer data sent, set read pointer and clear avail bytes */
265 _probe->ext_dma.dmapb.r_ptr += avail;
266 if (_probe->ext_dma.dmapb.r_ptr >= _probe->ext_dma.dmapb.end_addr)
267 _probe->ext_dma.dmapb.r_ptr -= _probe->ext_dma.dmapb.size;
268 _probe->ext_dma.dmapb.avail -= avail;
269
270 return SOF_TASK_STATE_RESCHEDULE;
271 }
272
probe_init(const struct probe_dma * probe_dma)273 int probe_init(const struct probe_dma *probe_dma)
274 {
275 struct probe_pdata *_probe = probe_get();
276 uint32_t i;
277 int err;
278
279 tr_dbg(&pr_tr, "probe_init()");
280
281 if (_probe) {
282 tr_err(&pr_tr, "probe_init(): Probes already initialized.");
283 return -EINVAL;
284 }
285
286 /* alloc probes main struct */
287 sof_get()->probe = rzalloc(SOF_MEM_ZONE_SYS_RUNTIME, 0, SOF_MEM_CAPS_RAM,
288 sizeof(*_probe));
289 _probe = probe_get();
290
291 if (!_probe) {
292 tr_err(&pr_tr, "probe_init(): Alloc failed.");
293 return -ENOMEM;
294 }
295
296 /* setup extraction dma if requested */
297 if (probe_dma) {
298 tr_dbg(&pr_tr, "\tstream_tag = %u, dma_buffer_size = %u",
299 probe_dma->stream_tag, probe_dma->dma_buffer_size);
300
301 _probe->ext_dma.stream_tag = probe_dma->stream_tag;
302 _probe->ext_dma.dma_buffer_size = probe_dma->dma_buffer_size;
303
304 err = probe_dma_init(&_probe->ext_dma, DMA_DIR_LMEM_TO_HMEM);
305 if (err < 0) {
306 tr_err(&pr_tr, "probe_init(): probe_dma_init() failed");
307 _probe->ext_dma.stream_tag = PROBE_DMA_INVALID;
308 return err;
309 }
310
311 err = dma_start_legacy(_probe->ext_dma.dc.chan);
312 if (err < 0) {
313 tr_err(&pr_tr, "probe_init(): failed to start extraction dma");
314
315 return -EBUSY;
316 }
317 /* init task for extraction probes */
318 schedule_task_init_ll(&_probe->dmap_work,
319 SOF_UUID(probe_task_uuid),
320 SOF_SCHEDULE_LL_TIMER, SOF_TASK_PRI_LOW,
321 probe_task, _probe, 0, 0);
322 } else {
323 tr_dbg(&pr_tr, "\tno extraction DMA setup");
324
325 _probe->ext_dma.stream_tag = PROBE_DMA_INVALID;
326 }
327
328 /* initialize injection DMAs as invalid */
329 for (i = 0; i < CONFIG_PROBE_DMA_MAX; i++)
330 _probe->inject_dma[i].stream_tag = PROBE_DMA_INVALID;
331
332 /* initialize probe points as invalid */
333 for (i = 0; i < CONFIG_PROBE_POINTS_MAX; i++)
334 _probe->probe_points[i].stream_tag = PROBE_POINT_INVALID;
335
336 return 0;
337 }
338
probe_deinit(void)339 int probe_deinit(void)
340 {
341 struct probe_pdata *_probe = probe_get();
342 uint32_t i;
343 int err;
344
345 tr_dbg(&pr_tr, "probe_deinit()");
346
347 if (!_probe) {
348 tr_err(&pr_tr, "probe_deinit(): Not initialized.");
349
350 return -EINVAL;
351 }
352
353 /* check for attached injection probe DMAs */
354 for (i = 0; i < CONFIG_PROBE_DMA_MAX; i++) {
355 if (_probe->inject_dma[i].stream_tag != PROBE_DMA_INVALID) {
356 tr_err(&pr_tr, "probe_deinit(): Cannot deinitialize with injection DMAs attached.");
357 return -EINVAL;
358 }
359 }
360
361 /* check for connected probe points */
362 for (i = 0; i < CONFIG_PROBE_POINTS_MAX; i++) {
363 if (_probe->probe_points[i].stream_tag != PROBE_POINT_INVALID) {
364 tr_err(&pr_tr, "probe_deinit(): Cannot deinitialize with probe points connected.");
365 return -EINVAL;
366 }
367 }
368
369 if (_probe->ext_dma.stream_tag != PROBE_DMA_INVALID) {
370 tr_dbg(&pr_tr, "probe_deinit() Freeing task and extraction DMA.");
371 schedule_task_free(&_probe->dmap_work);
372 err = probe_dma_deinit(&_probe->ext_dma);
373 if (err < 0)
374 return err;
375 }
376
377 sof_get()->probe = NULL;
378 rfree(_probe);
379
380 return 0;
381 }
382
probe_dma_add(uint32_t count,const struct probe_dma * probe_dma)383 int probe_dma_add(uint32_t count, const struct probe_dma *probe_dma)
384 {
385 struct probe_pdata *_probe = probe_get();
386 uint32_t i;
387 uint32_t j;
388 uint32_t stream_tag;
389 uint32_t first_free;
390 int err;
391
392 tr_dbg(&pr_tr, "probe_dma_add() count = %u", count);
393
394 if (!_probe) {
395 tr_err(&pr_tr, "probe_dma_add(): Not initialized.");
396
397 return -EINVAL;
398 }
399
400 /* Iterate over all (DMA) fields if there are multiple of them */
401 /* add them if there is free place and they are not already attached */
402 for (i = 0; i < count; i++) {
403 tr_dbg(&pr_tr, "\tprobe_dma[%u] stream_tag = %u, dma_buffer_size = %u",
404 i, probe_dma[i].stream_tag,
405 probe_dma[i].dma_buffer_size);
406
407 first_free = CONFIG_PROBE_DMA_MAX;
408
409 /* looking for first free dma slot */
410 for (j = 0; j < CONFIG_PROBE_DMA_MAX; j++) {
411 stream_tag = _probe->inject_dma[j].stream_tag;
412
413 if (stream_tag == PROBE_DMA_INVALID) {
414 if (first_free == CONFIG_PROBE_DMA_MAX)
415 first_free = j;
416
417 continue;
418 }
419
420 if (stream_tag == probe_dma[i].stream_tag) {
421 tr_err(&pr_tr, "probe_dma_add(): Probe DMA %u already attached.",
422 stream_tag);
423 return -EINVAL;
424 }
425 }
426
427 if (first_free == CONFIG_PROBE_DMA_MAX) {
428 tr_err(&pr_tr, "probe_dma_add(): Exceeded maximum number of DMAs attached = "
429 META_QUOTE(CONFIG_PROBE_DMA_MAX));
430 return -EINVAL;
431 }
432
433 _probe->inject_dma[first_free].stream_tag =
434 probe_dma[i].stream_tag;
435 _probe->inject_dma[first_free].dma_buffer_size =
436 probe_dma[i].dma_buffer_size;
437
438 err = probe_dma_init(&_probe->inject_dma[first_free],
439 DMA_DIR_HMEM_TO_LMEM);
440 if (err < 0) {
441 tr_err(&pr_tr, "probe_dma_add(): probe_dma_init() failed");
442 _probe->inject_dma[first_free].stream_tag =
443 PROBE_DMA_INVALID;
444 return err;
445 }
446 }
447
448 return 0;
449 }
450
451 /**
452 * \brief Check if stream_tag is used by probes.
453 * \param[in] stream_tag DMA stream tag.
454 * \return 0 if not used, 1 otherwise.
455 */
is_probe_stream_used(uint32_t stream_tag)456 static int is_probe_stream_used(uint32_t stream_tag)
457 {
458 struct probe_pdata *_probe = probe_get();
459 uint32_t i;
460
461 for (i = 0; i < CONFIG_PROBE_POINTS_MAX; i++) {
462 if (_probe->probe_points[i].stream_tag == stream_tag)
463 return 1;
464 }
465
466 return 0;
467 }
468
probe_dma_remove(uint32_t count,const uint32_t * stream_tag)469 int probe_dma_remove(uint32_t count, const uint32_t *stream_tag)
470 {
471 struct probe_pdata *_probe = probe_get();
472 uint32_t i;
473 uint32_t j;
474 int err;
475
476 tr_dbg(&pr_tr, "probe_dma_remove() count = %u", count);
477
478 if (!_probe) {
479 tr_err(&pr_tr, "probe_dma_remove(): Not initialized.");
480
481 return -EINVAL;
482 }
483
484 /* remove each DMA if they are not used */
485 for (i = 0; i < count; i++) {
486 tr_dbg(&pr_tr, "\tstream_tag[%u] = %u", i, stream_tag[i]);
487
488 if (is_probe_stream_used(stream_tag[i]))
489 return -EINVAL;
490
491 for (j = 0; j < CONFIG_PROBE_DMA_MAX; j++) {
492 if (_probe->inject_dma[j].stream_tag == stream_tag[i]) {
493 err = probe_dma_deinit(&_probe->inject_dma[j]);
494 if (err < 0)
495 return err;
496 }
497 }
498 }
499
500 return 0;
501 }
502
503 /**
504 * \brief Copy data to probe buffer and update buffer pointers.
505 * \param[out] pbuf DMA buffer.
506 * \param[in] data pointer.
507 * \param[in] bytes size.
508 * \return 0 on success, error code otherwise.
509 */
copy_to_pbuffer(struct probe_dma_buf * pbuf,void * data,uint32_t bytes)510 static int copy_to_pbuffer(struct probe_dma_buf *pbuf, void *data,
511 uint32_t bytes)
512 {
513 uint32_t head;
514 uint32_t tail;
515
516 if (bytes == 0)
517 return 0;
518
519 /* check if there is free room in probe buffer */
520 if (pbuf->size - pbuf->avail < bytes)
521 return -EINVAL;
522
523 /* check if it will not exceed end_addr */
524 if ((char *)pbuf->end_addr - (char *)pbuf->w_ptr < bytes) {
525 head = (char *)pbuf->end_addr - (char *)pbuf->w_ptr;
526 tail = bytes - head;
527 } else {
528 head = bytes;
529 tail = 0;
530 }
531
532 /* copy data to probe buffer */
533 if (memcpy_s((void *)pbuf->w_ptr, pbuf->end_addr - pbuf->w_ptr, data, head)) {
534 tr_err(&pr_tr, "copy_to_pbuffer(): memcpy_s() failed");
535 return -EINVAL;
536 }
537 dcache_writeback_region((__sparse_force void __sparse_cache *)pbuf->w_ptr, head);
538
539 /* buffer ended so needs to do a second copy */
540 if (tail) {
541 pbuf->w_ptr = pbuf->addr;
542 if (memcpy_s((void *)pbuf->w_ptr, (char *)pbuf->end_addr - (char *)pbuf->w_ptr,
543 (char *)data + head, tail)) {
544 tr_err(&pr_tr, "copy_to_pbuffer(): memcpy_s() failed");
545 return -EINVAL;
546 }
547 dcache_writeback_region((__sparse_force void __sparse_cache *)pbuf->w_ptr, tail);
548 pbuf->w_ptr = pbuf->w_ptr + tail;
549 } else {
550 pbuf->w_ptr = pbuf->w_ptr + head;
551 }
552
553 pbuf->avail = (uintptr_t)pbuf->avail + bytes;
554
555 return 0;
556 }
557
558 /**
559 * \brief Copy data from probe buffer and update buffer pointers.
560 * \param[out] pbuf DMA buffer.
561 * \param[out] data pointer.
562 * \param[in] bytes size.
563 * \return 0 on success, error code otherwise.
564 */
copy_from_pbuffer(struct probe_dma_buf * pbuf,void * data,uint32_t bytes)565 static int copy_from_pbuffer(struct probe_dma_buf *pbuf, void *data,
566 uint32_t bytes)
567 {
568 uint32_t head;
569 uint32_t tail;
570
571 if (bytes == 0)
572 return 0;
573 /* not enough data available so set it to 0 */
574 if (pbuf->avail < bytes) {
575 memset(data, 0, bytes);
576 return 0;
577 }
578
579 /* check if memcpy needs to be divided into two stages */
580 if (pbuf->end_addr - pbuf->r_ptr < bytes) {
581 head = pbuf->end_addr - pbuf->r_ptr;
582 tail = bytes - head;
583 } else {
584 head = bytes;
585 tail = 0;
586 }
587
588 /* data from DMA so invalidate it */
589 dcache_invalidate_region((__sparse_force void __sparse_cache *)pbuf->r_ptr, head);
590 if (memcpy_s(data, bytes, (void *)pbuf->r_ptr, head)) {
591 tr_err(&pr_tr, "copy_from_pbuffer(): memcpy_s() failed");
592 return -EINVAL;
593 }
594
595 /* second stage copy */
596 if (tail) {
597 /* starting from the beginning of the buffer */
598 pbuf->r_ptr = pbuf->addr;
599 dcache_invalidate_region((__sparse_force void __sparse_cache *)pbuf->r_ptr, tail);
600 if (memcpy_s((char *)data + head, tail, (void *)pbuf->r_ptr, tail)) {
601 tr_err(&pr_tr, "copy_from_pbuffer(): memcpy_s() failed");
602 return -EINVAL;
603 }
604 pbuf->r_ptr = pbuf->r_ptr + tail;
605 } else {
606 pbuf->r_ptr = pbuf->r_ptr + head;
607 }
608
609 /* subtract used bytes */
610 pbuf->avail -= bytes;
611
612 return 0;
613 }
614
615 /**
616 * \brief Generate probe data packet header, update timestamp, calc crc
617 * and copy data to probe buffer.
618 * \param[in] buffer_id component buffer id
619 * \param[in] size data size.
620 * \param[in] format audio format.
621 * \param[out] checksum.
622 * \return 0 on success, error code otherwise.
623 */
probe_gen_header(uint32_t buffer_id,uint32_t size,uint32_t format,uint64_t * checksum)624 static int probe_gen_header(uint32_t buffer_id, uint32_t size,
625 uint32_t format, uint64_t *checksum)
626 {
627 struct probe_pdata *_probe = probe_get();
628 struct probe_data_packet *header;
629 uint64_t timestamp;
630
631 header = &_probe->header;
632 timestamp = sof_cycle_get_64();
633
634 header->sync_word = PROBE_EXTRACT_SYNC_WORD;
635 header->buffer_id = buffer_id;
636 header->format = format;
637 header->timestamp_low = (uint32_t)timestamp;
638 header->timestamp_high = (uint32_t)(timestamp >> 32);
639 header->data_size_bytes = size;
640
641 /* calc checksum to check validation by probe parse app */
642 *checksum = header->sync_word +
643 header->buffer_id +
644 header->format +
645 header->timestamp_high +
646 header->timestamp_low +
647 header->data_size_bytes;
648
649 dcache_writeback_region((__sparse_force void __sparse_cache *)header, sizeof(*header));
650
651 return copy_to_pbuffer(&_probe->ext_dma.dmapb, header,
652 sizeof(struct probe_data_packet));
653 }
654
655 /**
656 * \brief Generate description of audio format for extraction probes.
657 * \param[in] frame_fmt format
658 * \param[in] rate sample rate.
659 * \param[in] channels number of channels
660 * \return format.
661 */
probe_gen_format(uint32_t frame_fmt,uint32_t rate,uint32_t channels)662 static uint32_t probe_gen_format(uint32_t frame_fmt, uint32_t rate,
663 uint32_t channels)
664 {
665 uint32_t format = 0;
666 uint32_t sample_rate;
667 uint32_t valid_bytes;
668 uint32_t container_bytes;
669 uint32_t float_fmt = 0;
670
671 switch (frame_fmt) {
672 case SOF_IPC_FRAME_S16_LE:
673 valid_bytes = 2;
674 container_bytes = 2;
675 break;
676 case SOF_IPC_FRAME_S24_4LE:
677 valid_bytes = 3;
678 container_bytes = 4;
679 break;
680 case SOF_IPC_FRAME_S32_LE:
681 valid_bytes = 4;
682 container_bytes = 4;
683 break;
684 case SOF_IPC_FRAME_FLOAT:
685 valid_bytes = 4;
686 container_bytes = 4;
687 float_fmt = 1;
688 break;
689 default:
690 tr_err(&pr_tr, "probe_gen_format(): Invalid frame format specified = 0x%08x",
691 frame_fmt);
692 assert(false);
693 return 0;
694 }
695
696 switch (rate) {
697 case 8000:
698 sample_rate = 0;
699 break;
700 case 11025:
701 sample_rate = 1;
702 break;
703 case 12000:
704 sample_rate = 2;
705 break;
706 case 16000:
707 sample_rate = 3;
708 break;
709 case 22050:
710 sample_rate = 4;
711 break;
712 case 24000:
713 sample_rate = 5;
714 break;
715 case 32000:
716 sample_rate = 6;
717 break;
718 case 44100:
719 sample_rate = 7;
720 break;
721 case 48000:
722 sample_rate = 8;
723 break;
724 case 64000:
725 sample_rate = 9;
726 break;
727 case 88200:
728 sample_rate = 10;
729 break;
730 case 96000:
731 sample_rate = 11;
732 break;
733 case 128000:
734 sample_rate = 12;
735 break;
736 case 176400:
737 sample_rate = 13;
738 break;
739 case 192000:
740 sample_rate = 14;
741 break;
742 default:
743 sample_rate = 15;
744 }
745
746 format |= (1 << PROBE_SHIFT_FMT_TYPE) & PROBE_MASK_FMT_TYPE;
747 format |= (sample_rate << PROBE_SHIFT_SAMPLE_RATE) & PROBE_MASK_SAMPLE_RATE;
748 format |= ((channels - 1) << PROBE_SHIFT_NB_CHANNELS) & PROBE_MASK_NB_CHANNELS;
749 format |= ((valid_bytes - 1) << PROBE_SHIFT_SAMPLE_SIZE) & PROBE_MASK_SAMPLE_SIZE;
750 format |= ((container_bytes - 1) << PROBE_SHIFT_CONTAINER_SIZE) & PROBE_MASK_CONTAINER_SIZE;
751 format |= (float_fmt << PROBE_SHIFT_SAMPLE_FMT) & PROBE_MASK_SAMPLE_FMT;
752 format |= (1 << PROBE_SHIFT_INTERLEAVING_ST) & PROBE_MASK_INTERLEAVING_ST;
753
754 return format;
755 }
756
757 /*
758 * Check if more than 75% of buffer is already used,
759 * and if yes, rescheduled the probe task immediately.
760 */
kick_probe_task(struct probe_pdata * _probe)761 static void kick_probe_task(struct probe_pdata *_probe)
762 {
763 if (_probe->ext_dma.dmapb.size - _probe->ext_dma.dmapb.avail <
764 _probe->ext_dma.dmapb.size >> 2)
765 reschedule_task(&_probe->dmap_work, 0);
766 }
767
768 #if CONFIG_LOG_BACKEND_SOF_PROBE
probe_logging_hook(uint8_t * buffer,size_t length)769 static void probe_logging_hook(uint8_t *buffer, size_t length)
770 {
771 struct probe_pdata *_probe = probe_get();
772 uint64_t checksum;
773 int ret;
774
775 ret = probe_gen_header(PROBE_LOGGING_BUFFER_ID, length, 0, &checksum);
776 if (ret < 0)
777 return;
778
779 ret = copy_to_pbuffer(&_probe->ext_dma.dmapb,
780 buffer, length);
781 if (ret < 0)
782 return;
783
784 ret = copy_to_pbuffer(&_probe->ext_dma.dmapb,
785 &checksum, sizeof(checksum));
786 if (ret < 0)
787 return;
788
789 kick_probe_task(_probe);
790 }
791 #endif
792
793 /**
794 * \brief General extraction probe callback, called from buffer produce.
795 * It will search for probe point connected to this buffer.
796 * Extraction probe: generate format, header and copy data to probe buffer.
797 * Injection probe: find corresponding DMA, check avail data, copy data,
798 * update pointers and request more data from host if needed.
799 * \param[in] arg pointer (not used).
800 * \param[in] type of notify.
801 * \param[in] data pointer.
802 */
probe_cb_produce(void * arg,enum notify_id type,void * data)803 static void probe_cb_produce(void *arg, enum notify_id type, void *data)
804 {
805 struct probe_pdata *_probe = probe_get();
806 struct buffer_cb_transact *cb_data = data;
807 struct comp_buffer __sparse_cache *buffer = cb_data->buffer;
808 struct probe_dma_ext *dma;
809 uint32_t buffer_id;
810 uint32_t head, tail;
811 uint32_t free_bytes = 0;
812 int32_t copy_bytes = 0;
813 int ret;
814 uint32_t i, j;
815 uint32_t format;
816 uint64_t checksum;
817
818 buffer_id = *(int *)arg;
819
820 /* search for probe point connected to this buffer */
821 for (i = 0; i < CONFIG_PROBE_POINTS_MAX; i++)
822 if (_probe->probe_points[i].buffer_id.full_id == buffer_id)
823 break;
824
825 if (i == CONFIG_PROBE_POINTS_MAX) {
826 tr_err(&pr_tr, "probe_cb_produce(): probe not found for buffer id: %d",
827 buffer_id);
828 return;
829 }
830
831 if (_probe->probe_points[i].purpose == PROBE_PURPOSE_EXTRACTION) {
832 format = probe_gen_format(buffer->stream.frame_fmt,
833 buffer->stream.rate,
834 buffer->stream.channels);
835 ret = probe_gen_header(buffer_id,
836 cb_data->transaction_amount,
837 format, &checksum);
838 if (ret < 0)
839 goto err;
840
841 /* check if transaction amount exceeds component buffer end addr */
842 /* if yes: divide copying into two stages, head and tail */
843 if ((char *)cb_data->transaction_begin_address +
844 cb_data->transaction_amount > (char *)buffer->stream.end_addr) {
845 head = (uintptr_t)buffer->stream.end_addr -
846 (uintptr_t)cb_data->transaction_begin_address;
847 tail = (uintptr_t)cb_data->transaction_amount - head;
848 ret = copy_to_pbuffer(&_probe->ext_dma.dmapb,
849 cb_data->transaction_begin_address,
850 head);
851 if (ret < 0)
852 goto err;
853
854 ret = copy_to_pbuffer(&_probe->ext_dma.dmapb,
855 buffer->stream.addr, tail);
856 if (ret < 0)
857 goto err;
858 } else {
859 ret = copy_to_pbuffer(&_probe->ext_dma.dmapb,
860 cb_data->transaction_begin_address,
861 cb_data->transaction_amount);
862 if (ret < 0)
863 goto err;
864 }
865
866 ret = copy_to_pbuffer(&_probe->ext_dma.dmapb,
867 &checksum, sizeof(checksum));
868 if (ret < 0)
869 goto err;
870
871 kick_probe_task(_probe);
872 } else {
873 /* search for DMA used by this probe point */
874 for (j = 0; j < CONFIG_PROBE_DMA_MAX; j++) {
875 if (_probe->inject_dma[j].stream_tag !=
876 PROBE_DMA_INVALID &&
877 _probe->inject_dma[j].stream_tag ==
878 _probe->probe_points[i].stream_tag) {
879 break;
880 }
881 }
882 if (j == CONFIG_PROBE_DMA_MAX) {
883 tr_err(&pr_tr, "probe_cb_produce(): dma not found");
884 return;
885 }
886 dma = &_probe->inject_dma[j];
887 /* get avail data info */
888 ret = dma_get_data_size_legacy(dma->dc.chan,
889 &dma->dmapb.avail,
890 &free_bytes);
891 if (ret < 0) {
892 tr_err(&pr_tr, "probe_cb_produce(): dma_get_data_size() failed, ret = %u",
893 ret);
894 goto err;
895 }
896
897 /* check if transaction amount exceeds component buffer end addr */
898 /* if yes: divide copying into two stages, head and tail */
899 if ((char *)cb_data->transaction_begin_address +
900 cb_data->transaction_amount > (char *)buffer->stream.end_addr) {
901 head = (char *)buffer->stream.end_addr -
902 (char *)cb_data->transaction_begin_address;
903 tail = cb_data->transaction_amount - head;
904
905 ret = copy_from_pbuffer(&dma->dmapb,
906 cb_data->transaction_begin_address, head);
907 if (ret < 0)
908 goto err;
909
910 ret = copy_from_pbuffer(&dma->dmapb,
911 buffer->stream.addr, tail);
912 if (ret < 0)
913 goto err;
914 } else {
915 ret = copy_from_pbuffer(&dma->dmapb,
916 cb_data->transaction_begin_address,
917 cb_data->transaction_amount);
918 if (ret < 0)
919 goto err;
920 }
921
922 /* calc how many data can be requested */
923 copy_bytes = dma->dmapb.r_ptr - dma->dmapb.w_ptr;
924 if (copy_bytes < 0)
925 copy_bytes += dma->dmapb.size;
926
927 /* align down to request at least 32 */
928 copy_bytes = ALIGN_DOWN(copy_bytes, 32);
929
930 /* check if copy_bytes is still valid for dma copy */
931 if (copy_bytes > 0) {
932 ret = dma_copy_to_host_nowait(&dma->dc,
933 &dma->config, 0,
934 (void *)dma->dmapb.r_ptr,
935 copy_bytes);
936 if (ret < 0)
937 goto err;
938
939 /* update pointers */
940 dma->dmapb.w_ptr = dma->dmapb.w_ptr + copy_bytes;
941 if (dma->dmapb.w_ptr > dma->dmapb.end_addr)
942 dma->dmapb.w_ptr = dma->dmapb.w_ptr - dma->dmapb.size;
943 }
944 }
945 return;
946 err:
947 tr_err(&pr_tr, "probe_cb_produce(): failed to generate probe data");
948 }
949
950 /**
951 * \brief Callback for buffer free, it will remove probe point.
952 * \param[in] arg pointer (not used).
953 * \param[in] type of notify.
954 * \param[in] data pointer.
955 */
probe_cb_free(void * arg,enum notify_id type,void * data)956 static void probe_cb_free(void *arg, enum notify_id type, void *data)
957 {
958 uint32_t buffer_id = *(int *)arg;
959 int ret;
960
961 tr_dbg(&pr_tr, "probe_cb_free() buffer_id = %u", buffer_id);
962
963 ret = probe_point_remove(1, &buffer_id);
964 if (ret < 0)
965 tr_err(&pr_tr, "probe_cb_free(): probe_point_remove() failed");
966 }
967
probe_purpose_needs_ext_dma(uint32_t purpose)968 static bool probe_purpose_needs_ext_dma(uint32_t purpose)
969 {
970 #if CONFIG_IPC_MAJOR_4
971 return purpose == PROBE_PURPOSE_EXTRACTION;
972 #else
973 return purpose == PROBE_PURPOSE_EXTRACTION || purpose == PROBE_PURPOSE_LOGGING;
974 #endif
975 }
976
977 #if CONFIG_IPC_MAJOR_4
ipc4_get_buffer(struct ipc_comp_dev * dev,probe_point_id_t probe_point)978 static struct comp_buffer *ipc4_get_buffer(struct ipc_comp_dev *dev, probe_point_id_t probe_point)
979 {
980 struct comp_buffer *buf;
981 struct comp_buffer __sparse_cache *buf_c;
982 struct list_item *sink_list, *source_list;
983 unsigned int queue_id;
984
985 switch (probe_point.fields.type) {
986 case PROBE_TYPE_INPUT:
987 list_for_item(source_list, &dev->cd->bsource_list) {
988 buf = container_of(source_list, struct comp_buffer, sink_list);
989 buf_c = buffer_acquire(buf);
990 queue_id = IPC4_SRC_QUEUE_ID(buf_c->id);
991 buffer_release(buf_c);
992
993 if (queue_id == probe_point.fields.index)
994 return buf;
995 }
996 break;
997 case PROBE_TYPE_OUTPUT:
998 list_for_item(sink_list, &dev->cd->bsink_list) {
999 buf = container_of(sink_list, struct comp_buffer, source_list);
1000 buf_c = buffer_acquire(buf);
1001 queue_id = IPC4_SINK_QUEUE_ID(buf_c->id);
1002 buffer_release(buf_c);
1003
1004 if (queue_id == probe_point.fields.index)
1005 return buf;
1006 }
1007 }
1008
1009 return NULL;
1010 }
1011 #endif
1012
enable_logs(const struct probe_point * probe)1013 static bool enable_logs(const struct probe_point *probe)
1014 {
1015 #if CONFIG_IPC_MAJOR_4
1016 return probe->buffer_id.fields.module_id == 0;
1017 #else
1018 return probe->purpose == PROBE_PURPOSE_LOGGING;
1019 #endif
1020 }
1021
verify_purpose(uint32_t purpose)1022 static bool verify_purpose(uint32_t purpose)
1023 {
1024 #if CONFIG_IPC_MAJOR_4
1025 return purpose == PROBE_PURPOSE_EXTRACTION ||
1026 purpose == PROBE_PURPOSE_INJECTION;
1027 #else
1028 return purpose == PROBE_PURPOSE_EXTRACTION ||
1029 purpose == PROBE_PURPOSE_INJECTION ||
1030 purpose == PROBE_PURPOSE_LOGGING;
1031 #endif
1032 }
1033
probe_point_add(uint32_t count,const struct probe_point * probe)1034 int probe_point_add(uint32_t count, const struct probe_point *probe)
1035 {
1036 struct probe_pdata *_probe = probe_get();
1037 uint32_t i;
1038 uint32_t j;
1039 uint32_t buffer_id;
1040 uint32_t first_free;
1041 uint32_t dma_found;
1042 uint32_t fw_logs;
1043 struct ipc_comp_dev *dev = NULL;
1044 #if CONFIG_IPC_MAJOR_4
1045 struct comp_buffer *buf = NULL;
1046 #endif
1047 tr_dbg(&pr_tr, "probe_point_add() count = %u", count);
1048
1049 if (!_probe) {
1050 tr_err(&pr_tr, "probe_point_add(): Not initialized.");
1051
1052 return -EINVAL;
1053 }
1054
1055 /* add all probe points if they are corresponding to valid component and DMA */
1056 for (i = 0; i < count; i++) {
1057 const probe_point_id_t *buf_id = &probe[i].buffer_id;
1058 uint32_t stream_tag;
1059
1060 tr_dbg(&pr_tr, "\tprobe[%u] buffer_id = %u, purpose = %u, stream_tag = %u",
1061 i, buf_id->full_id, probe[i].purpose,
1062 probe[i].stream_tag);
1063
1064 if (!verify_purpose(probe[i].purpose)) {
1065 tr_err(&pr_tr, "probe_point_add() error: invalid purpose %d",
1066 probe[i].purpose);
1067
1068 return -EINVAL;
1069 }
1070
1071 if (_probe->ext_dma.stream_tag == PROBE_DMA_INVALID &&
1072 probe_purpose_needs_ext_dma(probe[i].purpose)) {
1073 tr_err(&pr_tr, "probe_point_add(): extraction DMA not enabled.");
1074 return -EINVAL;
1075 }
1076
1077 fw_logs = enable_logs(&probe[i]);
1078
1079 if (!fw_logs && probe[i].purpose == PROBE_PURPOSE_EXTRACTION) {
1080 #if CONFIG_IPC_MAJOR_4
1081 dev = ipc_get_comp_by_id(ipc_get(),
1082 IPC4_COMP_ID(buf_id->fields.module_id,
1083 buf_id->fields.instance_id));
1084 #else
1085 dev = ipc_get_comp_by_id(ipc_get(), buf_id->full_id);
1086 #endif
1087 /* check if buffer exists */
1088 if (!dev) {
1089 tr_err(&pr_tr, "probe_point_add(): No device with ID %u found.",
1090 buf_id->full_id);
1091
1092 return -EINVAL;
1093 }
1094 #if CONFIG_IPC_MAJOR_4
1095 buf = ipc4_get_buffer(dev, *buf_id);
1096 if (!buf) {
1097 tr_err(&pr_tr, "probe_point_add(): buffer %u not found.",
1098 buf_id->full_id);
1099
1100 return -EINVAL;
1101 }
1102 #else
1103 if (dev->type != COMP_TYPE_BUFFER) {
1104 tr_err(&pr_tr, "probe_point_add(): Device ID %u is not a buffer.",
1105 buf_id->full_id);
1106
1107 return -EINVAL;
1108 }
1109 #endif
1110 }
1111
1112 first_free = CONFIG_PROBE_POINTS_MAX;
1113
1114 /* search for first free probe slot */
1115 for (j = 0; j < CONFIG_PROBE_POINTS_MAX; j++) {
1116 if (_probe->probe_points[j].stream_tag ==
1117 PROBE_POINT_INVALID) {
1118 if (first_free == CONFIG_PROBE_POINTS_MAX)
1119 first_free = j;
1120
1121 continue;
1122 }
1123 /* and check if probe is already attached */
1124 buffer_id = _probe->probe_points[j].buffer_id.full_id;
1125 if (buffer_id == buf_id->full_id) {
1126 if (_probe->probe_points[j].purpose ==
1127 probe[i].purpose) {
1128 tr_err(&pr_tr, "probe_point_add(): Probe already attached to buffer %u with purpose %u",
1129 buffer_id,
1130 probe[i].purpose);
1131
1132 return -EINVAL;
1133 }
1134 }
1135 }
1136
1137 if (first_free == CONFIG_PROBE_POINTS_MAX) {
1138 tr_err(&pr_tr, "probe_point_add(): Maximum number of probe points connected aleady: "
1139 META_QUOTE(CONFIG_PROBE_POINTS_MAX));
1140
1141 return -EINVAL;
1142 }
1143
1144 /* if connecting injection probe, check for associated DMA */
1145 if (probe[i].purpose == PROBE_PURPOSE_INJECTION) {
1146 dma_found = 0;
1147
1148 for (j = 0; j < CONFIG_PROBE_DMA_MAX; j++) {
1149 if (_probe->inject_dma[j].stream_tag !=
1150 PROBE_DMA_INVALID &&
1151 _probe->inject_dma[j].stream_tag ==
1152 probe[i].stream_tag) {
1153 dma_found = 1;
1154 break;
1155 }
1156 }
1157
1158 if (!dma_found) {
1159 tr_err(&pr_tr, "probe_point_add(): No DMA with stream tag %u found for injection.",
1160 probe[i].stream_tag);
1161
1162 return -EINVAL;
1163 }
1164 if (dma_start_legacy(_probe->inject_dma[j].dc.chan) < 0) {
1165 tr_err(&pr_tr, "probe_point_add(): failed to start dma");
1166
1167 return -EBUSY;
1168 }
1169
1170 stream_tag = probe[i].stream_tag;
1171 } else {
1172 /* prepare extraction DMA */
1173 for (j = 0; j < CONFIG_PROBE_POINTS_MAX; j++) {
1174 if (_probe->probe_points[j].stream_tag != PROBE_DMA_INVALID &&
1175 probe_purpose_needs_ext_dma(_probe->probe_points[j].purpose))
1176 break;
1177 }
1178
1179 if (j == CONFIG_PROBE_POINTS_MAX) {
1180 tr_dbg(&pr_tr, "probe_point_add(): start probe task");
1181 schedule_task(&_probe->dmap_work, 1000, 1000);
1182 }
1183 /* ignore probe stream tag for extraction probes */
1184 stream_tag = _probe->ext_dma.stream_tag;
1185 }
1186
1187 /* probe point valid, save it */
1188 _probe->probe_points[first_free].buffer_id = *buf_id;
1189 _probe->probe_points[first_free].purpose = probe[i].purpose;
1190 _probe->probe_points[first_free].stream_tag = stream_tag;
1191
1192 if (fw_logs) {
1193 #if CONFIG_LOG_BACKEND_SOF_PROBE
1194 probe_logging_init(probe_logging_hook);
1195 #else
1196 return -EINVAL;
1197 #endif
1198 } else {
1199 probe_point_id_t *new_buf_id = &_probe->probe_points[first_free].buffer_id;
1200
1201 #if CONFIG_IPC_MAJOR_4
1202 notifier_register(&new_buf_id->full_id, buf, NOTIFIER_ID_BUFFER_PRODUCE,
1203 &probe_cb_produce, 0);
1204 notifier_register(&new_buf_id->full_id, buf, NOTIFIER_ID_BUFFER_FREE,
1205 &probe_cb_free, 0);
1206 #else
1207 notifier_register(&new_buf_id->full_id, dev->cb, NOTIFIER_ID_BUFFER_PRODUCE,
1208 &probe_cb_produce, 0);
1209 notifier_register(&new_buf_id->full_id, dev->cb, NOTIFIER_ID_BUFFER_FREE,
1210 &probe_cb_free, 0);
1211 #endif
1212 }
1213 }
1214
1215 return 0;
1216 }
1217
1218 #if CONFIG_IPC_MAJOR_3
probe_dma_info(struct sof_ipc_probe_info_params * data,uint32_t max_size)1219 int probe_dma_info(struct sof_ipc_probe_info_params *data, uint32_t max_size)
1220 {
1221 struct probe_pdata *_probe = probe_get();
1222 uint32_t i = 0;
1223 uint32_t j = 0;
1224
1225 tr_dbg(&pr_tr, "probe_dma_info()");
1226
1227 if (!_probe) {
1228 tr_err(&pr_tr, "probe_dma_info(): Not initialized.");
1229
1230 return -EINVAL;
1231 }
1232
1233 data->rhdr.hdr.size = sizeof(*data);
1234
1235 /* search all injection DMAs to send them in reply */
1236 while (i < CONFIG_PROBE_DMA_MAX &&
1237 data->rhdr.hdr.size + sizeof(struct probe_dma) < max_size) {
1238 /* save it if valid */
1239 if (_probe->inject_dma[i].stream_tag != PROBE_DMA_INVALID) {
1240 data->probe_dma[j].stream_tag =
1241 _probe->inject_dma[i].stream_tag;
1242 data->probe_dma[j].dma_buffer_size =
1243 _probe->inject_dma[i].dma_buffer_size;
1244 j++;
1245 /* and increase reply header size */
1246 data->rhdr.hdr.size += sizeof(struct probe_dma);
1247 }
1248
1249 i++;
1250 }
1251
1252 data->num_elems = j;
1253
1254 return 1;
1255 }
1256
probe_point_info(struct sof_ipc_probe_info_params * data,uint32_t max_size)1257 int probe_point_info(struct sof_ipc_probe_info_params *data, uint32_t max_size)
1258 {
1259 struct probe_pdata *_probe = probe_get();
1260 uint32_t i = 0;
1261 uint32_t j = 0;
1262
1263 tr_dbg(&pr_tr, "probe_point_info()");
1264
1265 if (!_probe) {
1266 tr_err(&pr_tr, "probe_point_info(): Not initialized.");
1267
1268 return -EINVAL;
1269 }
1270
1271 data->rhdr.hdr.size = sizeof(*data);
1272 /* search for all probe points to send them in reply */
1273 while (i < CONFIG_PROBE_POINTS_MAX &&
1274 data->rhdr.hdr.size + sizeof(struct probe_point) < max_size) {
1275 if (_probe->probe_points[i].stream_tag != PROBE_POINT_INVALID) {
1276 data->probe_point[j].buffer_id =
1277 _probe->probe_points[i].buffer_id;
1278 data->probe_point[j].purpose =
1279 _probe->probe_points[i].purpose;
1280 data->probe_point[j].stream_tag =
1281 _probe->probe_points[i].stream_tag;
1282 j++;
1283 data->rhdr.hdr.size += sizeof(struct probe_point);
1284 }
1285
1286 i++;
1287 }
1288
1289 data->num_elems = j;
1290
1291 return 1;
1292 }
1293 #endif
1294
probe_point_remove(uint32_t count,const uint32_t * buffer_id)1295 int probe_point_remove(uint32_t count, const uint32_t *buffer_id)
1296 {
1297 struct probe_pdata *_probe = probe_get();
1298 struct ipc_comp_dev *dev;
1299 uint32_t i;
1300 uint32_t j;
1301 #if CONFIG_IPC_MAJOR_4
1302 struct comp_buffer *buf;
1303 #endif
1304
1305 tr_dbg(&pr_tr, "probe_point_remove() count = %u", count);
1306
1307 if (!_probe) {
1308 tr_err(&pr_tr, "probe_point_remove(): Not initialized.");
1309 return -EINVAL;
1310 }
1311 /* remove each requested probe point */
1312 for (i = 0; i < count; i++) {
1313 tr_dbg(&pr_tr, "\tbuffer_id[%u] = %u", i, buffer_id[i]);
1314
1315 for (j = 0; j < CONFIG_PROBE_POINTS_MAX; j++) {
1316 probe_point_id_t *buf_id = &_probe->probe_points[j].buffer_id;
1317
1318 if (_probe->probe_points[j].stream_tag != PROBE_POINT_INVALID &&
1319 buf_id->full_id == buffer_id[i]) {
1320 #if CONFIG_IPC_MAJOR_4
1321 dev = ipc_get_comp_by_id(ipc_get(),
1322 IPC4_COMP_ID(buf_id->fields.module_id,
1323 buf_id->fields.instance_id));
1324 if (dev) {
1325 buf = ipc4_get_buffer(dev, *buf_id);
1326 if (buf) {
1327 notifier_unregister(NULL, buf,
1328 NOTIFIER_ID_BUFFER_PRODUCE);
1329 notifier_unregister(NULL, buf,
1330 NOTIFIER_ID_BUFFER_FREE);
1331 }
1332 }
1333 #else
1334 dev = ipc_get_comp_by_id(ipc_get(), buffer_id[i]);
1335 if (dev) {
1336 notifier_unregister(_probe, dev->cb,
1337 NOTIFIER_ID_BUFFER_PRODUCE);
1338 notifier_unregister(_probe, dev->cb, NOTIFIER_ID_BUFFER_FREE);
1339 }
1340 #endif
1341 _probe->probe_points[j].stream_tag =
1342 PROBE_POINT_INVALID;
1343 }
1344 }
1345 }
1346 for (j = 0; j < CONFIG_PROBE_POINTS_MAX; j++) {
1347 if (_probe->probe_points[j].stream_tag != PROBE_DMA_INVALID &&
1348 _probe->probe_points[j].purpose == PROBE_PURPOSE_EXTRACTION)
1349 break;
1350 }
1351 if (j == CONFIG_PROBE_POINTS_MAX) {
1352 tr_dbg(&pr_tr, "probe_point_remove(): cancel probe task");
1353 schedule_task_cancel(&_probe->dmap_work);
1354 }
1355
1356 return 0;
1357 }
1358
1359 #if CONFIG_IPC_MAJOR_4
probe_new(const struct comp_driver * drv,const struct comp_ipc_config * config,const void * spec)1360 static struct comp_dev *probe_new(const struct comp_driver *drv,
1361 const struct comp_ipc_config *config, const void *spec)
1362 {
1363 const struct ipc4_probe_module_cfg *probe_cfg = spec;
1364 struct comp_dev *dev;
1365 int ret;
1366
1367 comp_cl_info(&comp_probe, "probe_new()");
1368
1369 dev = comp_alloc(drv, sizeof(*dev));
1370 if (!dev)
1371 return NULL;
1372
1373 dev->ipc_config = *config;
1374
1375 ret = probe_init(&probe_cfg->gtw_cfg);
1376 if (ret < 0) {
1377 comp_free(dev);
1378 return NULL;
1379 }
1380 dev->state = COMP_STATE_READY;
1381
1382 return dev;
1383 }
1384
probe_free(struct comp_dev * dev)1385 static void probe_free(struct comp_dev *dev)
1386 {
1387 probe_deinit();
1388 rfree(dev);
1389 }
1390
probe_set_large_config(struct comp_dev * dev,uint32_t param_id,bool first_block,bool last_block,uint32_t data_offset,const char * data)1391 static int probe_set_large_config(struct comp_dev *dev, uint32_t param_id,
1392 bool first_block,
1393 bool last_block,
1394 uint32_t data_offset,
1395 const char *data)
1396 {
1397 comp_dbg(dev, "probe_set_large_config()");
1398
1399 switch (param_id) {
1400 case IPC4_PROBE_MODULE_PROBE_POINTS_ADD:
1401 return probe_point_add(data_offset / sizeof(struct probe_point),
1402 (const struct probe_point *)data);
1403 case IPC4_PROBE_MODULE_DISCONNECT_PROBE_POINTS:
1404 return probe_point_remove(data_offset / sizeof(uint32_t), (const uint32_t *)data);
1405 case IPC4_PROBE_MODULE_INJECTION_DMA_ADD:
1406 return probe_dma_add(data_offset / (2 * sizeof(uint32_t)),
1407 (const struct probe_dma *)data);
1408 case IPC4_PROBE_MODULE_INJECTION_DMA_DETACH:
1409 return probe_dma_remove(data_offset / sizeof(uint32_t), (const uint32_t *)data);
1410 default:
1411 return -EINVAL;
1412 }
1413 }
1414
probe_get_large_config(struct comp_dev * dev,uint32_t param_id,bool first_block,bool last_block,uint32_t * data_offset,char * data)1415 static int probe_get_large_config(struct comp_dev *dev, uint32_t param_id,
1416 bool first_block,
1417 bool last_block,
1418 uint32_t *data_offset,
1419 char *data)
1420 {
1421 return 0;
1422 }
1423
1424 static const struct comp_driver comp_probe = {
1425 .uid = SOF_RT_UUID(probe_uuid),
1426 .tctx = &pr_tr,
1427 .ops = {
1428 .create = probe_new,
1429 .free = probe_free,
1430 .set_large_config = probe_set_large_config,
1431 .get_large_config = probe_get_large_config,
1432 },
1433 };
1434
1435 static SHARED_DATA struct comp_driver_info comp_probe_info = {
1436 .drv = &comp_probe,
1437 };
1438
sys_comp_probe_init(void)1439 UT_STATIC void sys_comp_probe_init(void)
1440 {
1441 comp_register(platform_shared_get(&comp_probe_info,
1442 sizeof(comp_probe_info)));
1443 }
1444
1445 DECLARE_MODULE(sys_comp_probe_init);
1446 SOF_MODULE_INIT(probe, sys_comp_probe_init);
1447 #endif
1448
