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 // 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 <sof/lib/alloc.h>
14 #include <sof/lib/dma.h>
15 #include <sof/lib/notifier.h>
16 #include <sof/lib/uuid.h>
17 #include <sof/ipc/topology.h>
18 #include <sof/ipc/driver.h>
19 #include <sof/drivers/timer.h>
20 #include <sof/schedule/ll_schedule.h>
21 #include <sof/schedule/schedule.h>
22 #include <sof/schedule/task.h>
23
24 /* 9d1fb66e-4ffb-497f-994b-17719686596e */
25 DECLARE_SOF_UUID("probe", probe_uuid, 0x9d1fb66e, 0x4ffb, 0x497f,
26 0x99, 0x4b, 0x17, 0x71, 0x96, 0x86, 0x59, 0x6e);
27
28 DECLARE_TR_CTX(pr_tr, SOF_UUID(probe_uuid), LOG_LEVEL_INFO);
29
30 /* 2f0b1901-cac0-4b87-812f-f2d5e4f19e4a */
31 DECLARE_SOF_UUID("probe-task", probe_task_uuid, 0x2f0b1901, 0xcac0, 0x4b87,
32 0x81, 0x2f, 0xf2, 0xd5, 0xe4, 0xf1, 0x9e, 0x4a);
33
34 #define PROBE_DMA_INVALID 0xFFFFFFFF
35 #define PROBE_POINT_INVALID 0xFFFFFFFF
36
37 #define PROBE_BUFFER_LOCAL_SIZE 8192
38 #define DMA_ELEM_SIZE 32
39
40 /**
41 * DMA buffer
42 */
43 struct probe_dma_buf {
44 uintptr_t w_ptr; /**< write pointer */
45 uintptr_t r_ptr; /**< read pointer */
46 uintptr_t addr; /**< buffer start pointer */
47 uintptr_t end_addr; /**< buffer end pointer */
48 uint32_t size; /**< buffer size */
49 uint32_t avail; /**< buffer avail data */
50 };
51
52 /**
53 * Probe DMA
54 */
55 struct probe_dma_ext {
56 uint32_t stream_tag; /**< DMA stream tag */
57 uint32_t dma_buffer_size; /**< DMA buffer size */
58 struct dma_sg_config config; /**< DMA SG config */
59 struct probe_dma_buf dmapb; /**< DMA buffer pointer */
60 struct dma_copy dc; /**< DMA copy */
61 };
62
63 /**
64 * Probe main struct
65 */
66 struct probe_pdata {
67 struct probe_dma_ext ext_dma; /**< extraction DMA */
68 struct probe_dma_ext inject_dma[CONFIG_PROBE_DMA_MAX]; /**< injection DMA */
69 struct probe_point probe_points[CONFIG_PROBE_POINTS_MAX]; /**< probe points */
70 struct probe_data_packet header; /**< data packet header */
71 struct task dmap_work; /**< probe task */
72 };
73
74 /**
75 * \brief Allocate and initialize probe buffer with correct alignment.
76 * \param[out] buffer return value.
77 * \param[in] size of buffer.
78 * \param[in] align the buffer.
79 * \return 0 on success, error code otherwise.
80 */
probe_dma_buffer_init(struct probe_dma_buf * buffer,uint32_t size,uint32_t align)81 static int probe_dma_buffer_init(struct probe_dma_buf *buffer, uint32_t size,
82 uint32_t align)
83 {
84 /* allocate new buffer */
85 buffer->addr = (uintptr_t)rballoc_align(0, SOF_MEM_CAPS_DMA,
86 size, align);
87
88 if (!buffer->addr) {
89 tr_err(&pr_tr, "probe_dma_buffer_init(): alloc failed");
90 return -ENOMEM;
91 }
92
93 bzero((void *)buffer->addr, size);
94 dcache_writeback_region((void *)buffer->addr, size);
95
96 /* initialise the DMA buffer */
97 buffer->size = size;
98 buffer->w_ptr = buffer->addr;
99 buffer->r_ptr = buffer->addr;
100 buffer->end_addr = buffer->addr + buffer->size;
101 buffer->avail = 0;
102
103 return 0;
104 }
105
106 /**
107 * \brief Request DMA and initialize DMA for probes with correct alignment,
108 * size and specific channel.
109 *
110 * \param[out] dma probe returned
111 * \param[in] direction of the DMA
112 * \return 0 on success, error code otherwise.
113 */
probe_dma_init(struct probe_dma_ext * dma,uint32_t direction)114 static int probe_dma_init(struct probe_dma_ext *dma, uint32_t direction)
115 {
116 struct dma_sg_config config;
117 uint32_t elem_addr, addr_align;
118 const uint32_t elem_size = sizeof(uint64_t) * DMA_ELEM_SIZE;
119 const uint32_t elem_num = PROBE_BUFFER_LOCAL_SIZE / elem_size;
120 int err = 0;
121
122 /* request DMA in the dir LMEM->HMEM with shared access */
123 dma->dc.dmac = dma_get(direction, 0, DMA_DEV_HOST,
124 DMA_ACCESS_SHARED);
125 if (!dma->dc.dmac) {
126 tr_err(&pr_tr, "probe_dma_init(): dma->dc.dmac = NULL");
127 return -ENODEV;
128 }
129
130 /* get required address alignment for dma buffer */
131 err = dma_get_attribute(dma->dc.dmac, DMA_ATTR_BUFFER_ADDRESS_ALIGNMENT,
132 &addr_align);
133 if (err < 0)
134 return err;
135
136 /* initialize dma buffer */
137 err = probe_dma_buffer_init(&dma->dmapb, PROBE_BUFFER_LOCAL_SIZE, addr_align);
138 if (err < 0)
139 return err;
140
141 err = dma_copy_set_stream_tag(&dma->dc, dma->stream_tag);
142 if (err < 0)
143 return err;
144
145 elem_addr = (uint32_t)dma->dmapb.addr;
146
147 config.direction = direction;
148 config.src_width = sizeof(uint32_t);
149 config.dest_width = sizeof(uint32_t);
150 config.cyclic = 0;
151
152 err = dma_sg_alloc(&config.elem_array, SOF_MEM_ZONE_RUNTIME,
153 config.direction, elem_num, elem_size, elem_addr, 0);
154 if (err < 0)
155 return err;
156
157 err = dma_set_config(dma->dc.chan, &config);
158 if (err < 0)
159 return err;
160
161 dma_sg_free(&config.elem_array);
162
163 return 0;
164 }
165
166 /**
167 * \brief Stop, deinit and free DMA and buffer used by probes.
168 *
169 * \return 0 on success, error code otherwise.
170 */
probe_dma_deinit(struct probe_dma_ext * dma)171 static int probe_dma_deinit(struct probe_dma_ext *dma)
172 {
173 int err = 0;
174
175 err = dma_stop(dma->dc.chan);
176 if (err < 0) {
177 tr_err(&pr_tr, "probe_dma_deinit(): dma_stop() failed");
178 return err;
179 }
180
181 dma_channel_put(dma->dc.chan);
182 dma_put(dma->dc.dmac);
183
184 rfree((void *)dma->dmapb.addr);
185 dma->dmapb.addr = 0;
186
187 dma->stream_tag = PROBE_DMA_INVALID;
188
189 return 0;
190 }
191
192 /*
193 * \brief Probe task for extraction.
194 *
195 * Copy extraction probes data to host if available.
196 * Return err if dma copy failed.
197 */
probe_task(void * data)198 static enum task_state probe_task(void *data)
199 {
200 struct probe_pdata *_probe = probe_get();
201 int err;
202
203 if (_probe->ext_dma.dmapb.avail > 0)
204 err = dma_copy_to_host_nowait(&_probe->ext_dma.dc,
205 &_probe->ext_dma.config, 0,
206 (void *)_probe->ext_dma.dmapb.r_ptr,
207 _probe->ext_dma.dmapb.avail);
208 else
209 return SOF_TASK_STATE_RESCHEDULE;
210
211 if (err < 0) {
212 tr_err(&pr_tr, "probe_task(): dma_copy_to_host_nowait() failed.");
213 return err;
214 }
215
216 /* buffer data sent, set read pointer and clear avail bytes */
217 _probe->ext_dma.dmapb.r_ptr = _probe->ext_dma.dmapb.w_ptr;
218 _probe->ext_dma.dmapb.avail = 0;
219
220 return SOF_TASK_STATE_RESCHEDULE;
221 }
222
probe_init(struct probe_dma * probe_dma)223 int probe_init(struct probe_dma *probe_dma)
224 {
225 struct probe_pdata *_probe = probe_get();
226 uint32_t i;
227 int err;
228
229 tr_dbg(&pr_tr, "probe_init()");
230
231 if (_probe) {
232 tr_err(&pr_tr, "probe_init(): Probes already initialized.");
233 return -EINVAL;
234 }
235
236 /* alloc probes main struct */
237 sof_get()->probe = rzalloc(SOF_MEM_ZONE_SYS_RUNTIME, 0, SOF_MEM_CAPS_RAM,
238 sizeof(*_probe));
239 _probe = probe_get();
240
241 if (!_probe) {
242 tr_err(&pr_tr, "probe_init(): Alloc failed.");
243 return -ENOMEM;
244 }
245
246 /* setup extraction dma if requested */
247 if (probe_dma) {
248 tr_dbg(&pr_tr, "\tstream_tag = %u, dma_buffer_size = %u",
249 probe_dma->stream_tag, probe_dma->dma_buffer_size);
250
251 _probe->ext_dma.stream_tag = probe_dma->stream_tag;
252 _probe->ext_dma.dma_buffer_size = probe_dma->dma_buffer_size;
253
254 err = probe_dma_init(&_probe->ext_dma, DMA_DIR_LMEM_TO_HMEM);
255 if (err < 0) {
256 tr_err(&pr_tr, "probe_init(): probe_dma_init() failed");
257 _probe->ext_dma.stream_tag = PROBE_DMA_INVALID;
258 return err;
259 }
260
261 err = dma_start(_probe->ext_dma.dc.chan);
262 if (err < 0) {
263 tr_err(&pr_tr, "probe_init(): failed to start extraction dma");
264
265 return -EBUSY;
266 }
267 /* init task for extraction probes */
268 schedule_task_init_ll(&_probe->dmap_work,
269 SOF_UUID(probe_task_uuid),
270 SOF_SCHEDULE_LL_TIMER, SOF_TASK_PRI_LOW,
271 probe_task, _probe, 0, 0);
272 } else {
273 tr_dbg(&pr_tr, "\tno extraction DMA setup");
274
275 _probe->ext_dma.stream_tag = PROBE_DMA_INVALID;
276 }
277
278 /* initialize injection DMAs as invalid */
279 for (i = 0; i < CONFIG_PROBE_DMA_MAX; i++)
280 _probe->inject_dma[i].stream_tag = PROBE_DMA_INVALID;
281
282 /* initialize probe points as invalid */
283 for (i = 0; i < CONFIG_PROBE_POINTS_MAX; i++)
284 _probe->probe_points[i].stream_tag = PROBE_POINT_INVALID;
285
286 return 0;
287 }
288
probe_deinit(void)289 int probe_deinit(void)
290 {
291 struct probe_pdata *_probe = probe_get();
292 uint32_t i;
293 int err;
294
295 tr_dbg(&pr_tr, "probe_deinit()");
296
297 if (!_probe) {
298 tr_err(&pr_tr, "probe_deinit(): Not initialized.");
299
300 return -EINVAL;
301 }
302
303 /* check for attached injection probe DMAs */
304 for (i = 0; i < CONFIG_PROBE_DMA_MAX; i++) {
305 if (_probe->inject_dma[i].stream_tag != PROBE_DMA_INVALID) {
306 tr_err(&pr_tr, "probe_deinit(): Cannot deinitialize with injection DMAs attached.");
307 return -EINVAL;
308 }
309 }
310
311 /* check for connected probe points */
312 for (i = 0; i < CONFIG_PROBE_POINTS_MAX; i++) {
313 if (_probe->probe_points[i].stream_tag != PROBE_POINT_INVALID) {
314 tr_err(&pr_tr, "probe_deinit(): Cannot deinitialize with probe points connected.");
315 return -EINVAL;
316 }
317 }
318
319 if (_probe->ext_dma.stream_tag != PROBE_DMA_INVALID) {
320 tr_dbg(&pr_tr, "probe_deinit() Freeing task and extraction DMA.");
321 schedule_task_free(&_probe->dmap_work);
322 err = probe_dma_deinit(&_probe->ext_dma);
323 if (err < 0)
324 return err;
325 }
326
327 sof_get()->probe = NULL;
328 rfree(_probe);
329
330 return 0;
331 }
332
probe_dma_add(uint32_t count,struct probe_dma * probe_dma)333 int probe_dma_add(uint32_t count, struct probe_dma *probe_dma)
334 {
335 struct probe_pdata *_probe = probe_get();
336 uint32_t i;
337 uint32_t j;
338 uint32_t stream_tag;
339 uint32_t first_free;
340 int err;
341
342 tr_dbg(&pr_tr, "probe_dma_add() count = %u", count);
343
344 if (!_probe) {
345 tr_err(&pr_tr, "probe_dma_add(): Not initialized.");
346
347 return -EINVAL;
348 }
349
350 /* Iterate over all (DMA) fields if there are multiple of them */
351 /* add them if there is free place and they are not already attached */
352 for (i = 0; i < count; i++) {
353 tr_dbg(&pr_tr, "\tprobe_dma[%u] stream_tag = %u, dma_buffer_size = %u",
354 i, probe_dma[i].stream_tag,
355 probe_dma[i].dma_buffer_size);
356
357 first_free = CONFIG_PROBE_DMA_MAX;
358
359 /* looking for first free dma slot */
360 for (j = 0; j < CONFIG_PROBE_DMA_MAX; j++) {
361 stream_tag = _probe->inject_dma[j].stream_tag;
362
363 if (stream_tag == PROBE_DMA_INVALID) {
364 if (first_free == CONFIG_PROBE_DMA_MAX)
365 first_free = j;
366
367 continue;
368 }
369
370 if (stream_tag == probe_dma[i].stream_tag) {
371 tr_err(&pr_tr, "probe_dma_add(): Probe DMA %u already attached.",
372 stream_tag);
373 return -EINVAL;
374 }
375 }
376
377 if (first_free == CONFIG_PROBE_DMA_MAX) {
378 tr_err(&pr_tr, "probe_dma_add(): Exceeded maximum number of DMAs attached = "
379 META_QUOTE(CONFIG_PROBE_DMA_MAX));
380 return -EINVAL;
381 }
382
383 _probe->inject_dma[first_free].stream_tag =
384 probe_dma[i].stream_tag;
385 _probe->inject_dma[first_free].dma_buffer_size =
386 probe_dma[i].dma_buffer_size;
387
388 err = probe_dma_init(&_probe->inject_dma[first_free],
389 DMA_DIR_HMEM_TO_LMEM);
390 if (err < 0) {
391 tr_err(&pr_tr, "probe_dma_add(): probe_dma_init() failed");
392 _probe->inject_dma[first_free].stream_tag =
393 PROBE_DMA_INVALID;
394 return err;
395 }
396 }
397
398 return 0;
399 }
400
probe_dma_info(struct sof_ipc_probe_info_params * data,uint32_t max_size)401 int probe_dma_info(struct sof_ipc_probe_info_params *data, uint32_t max_size)
402 {
403 struct probe_pdata *_probe = probe_get();
404 uint32_t i = 0;
405 uint32_t j = 0;
406
407 tr_dbg(&pr_tr, "probe_dma_info()");
408
409 if (!_probe) {
410 tr_err(&pr_tr, "probe_dma_info(): Not initialized.");
411
412 return -EINVAL;
413 }
414
415 data->rhdr.hdr.size = sizeof(*data);
416
417 /* search all injection DMAs to send them in reply */
418 while (i < CONFIG_PROBE_DMA_MAX &&
419 data->rhdr.hdr.size + sizeof(struct probe_dma) < max_size) {
420 /* save it if valid */
421 if (_probe->inject_dma[i].stream_tag != PROBE_DMA_INVALID) {
422 data->probe_dma[j].stream_tag =
423 _probe->inject_dma[i].stream_tag;
424 data->probe_dma[j].dma_buffer_size =
425 _probe->inject_dma[i].dma_buffer_size;
426 j++;
427 /* and increase reply header size */
428 data->rhdr.hdr.size += sizeof(struct probe_dma);
429 }
430
431 i++;
432 }
433
434 data->num_elems = j;
435
436 return 1;
437 }
438
439 /**
440 * \brief Check if stream_tag is used by probes.
441 * \param[in] stream_tag DMA stream tag.
442 * \return 0 if not used, 1 otherwise.
443 */
is_probe_stream_used(uint32_t stream_tag)444 static int is_probe_stream_used(uint32_t stream_tag)
445 {
446 struct probe_pdata *_probe = probe_get();
447 uint32_t i;
448
449 for (i = 0; i < CONFIG_PROBE_POINTS_MAX; i++) {
450 if (_probe->probe_points[i].stream_tag == stream_tag)
451 return 1;
452 }
453
454 return 0;
455 }
456
probe_dma_remove(uint32_t count,uint32_t * stream_tag)457 int probe_dma_remove(uint32_t count, uint32_t *stream_tag)
458 {
459 struct probe_pdata *_probe = probe_get();
460 uint32_t i;
461 uint32_t j;
462 int err;
463
464 tr_dbg(&pr_tr, "probe_dma_remove() count = %u", count);
465
466 if (!_probe) {
467 tr_err(&pr_tr, "probe_dma_remove(): Not initialized.");
468
469 return -EINVAL;
470 }
471
472 /* remove each DMA if they are not used */
473 for (i = 0; i < count; i++) {
474 tr_dbg(&pr_tr, "\tstream_tag[%u] = %u", i, stream_tag[i]);
475
476 if (is_probe_stream_used(stream_tag[i]))
477 return -EINVAL;
478
479 for (j = 0; j < CONFIG_PROBE_DMA_MAX; j++) {
480 if (_probe->inject_dma[j].stream_tag == stream_tag[i]) {
481 err = probe_dma_deinit(&_probe->inject_dma[j]);
482 if (err < 0)
483 return err;
484 }
485 }
486 }
487
488 return 0;
489 }
490
491 /**
492 * \brief Copy data to probe buffer and update buffer pointers.
493 * \param[out] pbuf DMA buffer.
494 * \param[in] data pointer.
495 * \param[in] bytes size.
496 * \return 0 on success, error code otherwise.
497 */
copy_to_pbuffer(struct probe_dma_buf * pbuf,void * data,uint32_t bytes)498 static int copy_to_pbuffer(struct probe_dma_buf *pbuf, void *data,
499 uint32_t bytes)
500 {
501 uint32_t head;
502 uint32_t tail;
503
504 if (bytes == 0)
505 return 0;
506
507 /* check if it will not exceed end_addr */
508 if ((char *)pbuf->end_addr - (char *)pbuf->w_ptr < bytes) {
509 head = (char *)pbuf->end_addr - (char *)pbuf->w_ptr;
510 tail = bytes - head;
511 } else {
512 head = bytes;
513 tail = 0;
514 }
515
516 /* copy data to probe buffer */
517 if (memcpy_s((void *)pbuf->w_ptr, pbuf->end_addr - pbuf->w_ptr, data, head)) {
518 tr_err(&pr_tr, "copy_to_pbuffer(): memcpy_s() failed");
519 return -EINVAL;
520 }
521 dcache_writeback_region((void *)pbuf->w_ptr, head);
522
523 /* buffer ended so needs to do a second copy */
524 if (tail) {
525 pbuf->w_ptr = pbuf->addr;
526 if (memcpy_s((void *)pbuf->w_ptr, (char *)pbuf->end_addr - (char *)pbuf->w_ptr,
527 (char *)data + head, tail)) {
528 tr_err(&pr_tr, "copy_to_pbuffer(): memcpy_s() failed");
529 return -EINVAL;
530 }
531 dcache_writeback_region((void *)pbuf->w_ptr, tail);
532 pbuf->w_ptr = pbuf->w_ptr + tail;
533 } else {
534 pbuf->w_ptr = pbuf->w_ptr + head;
535 }
536
537 pbuf->avail = (uintptr_t)pbuf->avail + bytes;
538
539 return 0;
540 }
541
542 /**
543 * \brief Copy data from probe buffer and update buffer pointers.
544 * \param[out] pbuf DMA buffer.
545 * \param[out] data pointer.
546 * \param[in] bytes size.
547 * \return 0 on success, error code otherwise.
548 */
copy_from_pbuffer(struct probe_dma_buf * pbuf,void * data,uint32_t bytes)549 static int copy_from_pbuffer(struct probe_dma_buf *pbuf, void *data,
550 uint32_t bytes)
551 {
552 uint32_t head;
553 uint32_t tail;
554
555 if (bytes == 0)
556 return 0;
557 /* not enough data available so set it to 0 */
558 if (pbuf->avail < bytes) {
559 memset(data, 0, bytes);
560 return 0;
561 }
562
563 /* check if memcpy needs to be divided into two stages */
564 if (pbuf->end_addr - pbuf->r_ptr < bytes) {
565 head = pbuf->end_addr - pbuf->r_ptr;
566 tail = bytes - head;
567 } else {
568 head = bytes;
569 tail = 0;
570 }
571
572 /* data from DMA so invalidate it */
573 dcache_invalidate_region((void *)pbuf->r_ptr, head);
574 if (memcpy_s(data, bytes, (void *)pbuf->r_ptr, head)) {
575 tr_err(&pr_tr, "copy_from_pbuffer(): memcpy_s() failed");
576 return -EINVAL;
577 }
578
579 /* second stage copy */
580 if (tail) {
581 /* starting from the beginning of the buffer */
582 pbuf->r_ptr = pbuf->addr;
583 dcache_invalidate_region((void *)pbuf->r_ptr, tail);
584 if (memcpy_s((char *)data + head, tail, (void *)pbuf->r_ptr, tail)) {
585 tr_err(&pr_tr, "copy_from_pbuffer(): memcpy_s() failed");
586 return -EINVAL;
587 }
588 pbuf->r_ptr = pbuf->r_ptr + tail;
589 } else {
590 pbuf->r_ptr = pbuf->r_ptr + head;
591 }
592
593 /* subtract used bytes */
594 pbuf->avail -= bytes;
595
596 return 0;
597 }
598
599 /**
600 * \brief Generate probe data packet header, update timestamp, calc crc
601 * and copy data to probe buffer.
602 * \param[in] buffer component buffer pointer.
603 * \param[in] size data size.
604 * \param[in] format audio format.
605 * \return 0 on success, error code otherwise.
606 */
probe_gen_header(struct comp_buffer * buffer,uint32_t size,uint32_t format)607 static int probe_gen_header(struct comp_buffer *buffer, uint32_t size,
608 uint32_t format)
609 {
610 struct probe_pdata *_probe = probe_get();
611 struct probe_data_packet *header;
612 uint64_t timestamp;
613 uint32_t crc;
614
615 header = &_probe->header;
616 timestamp = platform_timer_get(timer_get());
617
618 header->sync_word = PROBE_EXTRACT_SYNC_WORD;
619 header->buffer_id = buffer->id;
620 header->format = format;
621 header->timestamp_low = (uint32_t)timestamp;
622 header->timestamp_high = (uint32_t)(timestamp >> 32);
623 header->checksum = 0;
624 header->data_size_bytes = size;
625
626 /* calc crc to check validation by probe parse app */
627 crc = crc32(0, header, sizeof(*header));
628 header->checksum = crc;
629
630 dcache_writeback_region(header, sizeof(*header));
631
632 return copy_to_pbuffer(&_probe->ext_dma.dmapb, header,
633 sizeof(struct probe_data_packet));
634 }
635
636 /**
637 * \brief Generate description of audio format for extraction probes.
638 * \param[in] frame_fmt format
639 * \param[in] rate sample rate.
640 * \param[in] channels number of channels
641 * \return format.
642 */
probe_gen_format(uint32_t frame_fmt,uint32_t rate,uint32_t channels)643 static uint32_t probe_gen_format(uint32_t frame_fmt, uint32_t rate,
644 uint32_t channels)
645 {
646 uint32_t format = 0;
647 uint32_t sample_rate;
648 uint32_t valid_bytes;
649 uint32_t container_bytes;
650 uint32_t float_fmt = 0;
651
652 switch (frame_fmt) {
653 case SOF_IPC_FRAME_S16_LE:
654 valid_bytes = 2;
655 container_bytes = 2;
656 break;
657 case SOF_IPC_FRAME_S24_4LE:
658 valid_bytes = 3;
659 container_bytes = 4;
660 break;
661 case SOF_IPC_FRAME_S32_LE:
662 valid_bytes = 4;
663 container_bytes = 4;
664 break;
665 case SOF_IPC_FRAME_FLOAT:
666 valid_bytes = 4;
667 container_bytes = 4;
668 float_fmt = 1;
669 break;
670 default:
671 tr_err(&pr_tr, "probe_gen_format(): Invalid frame format specified = 0x%08x",
672 frame_fmt);
673 assert(false);
674 }
675
676 switch (rate) {
677 case 8000:
678 sample_rate = 0;
679 break;
680 case 11025:
681 sample_rate = 1;
682 break;
683 case 12000:
684 sample_rate = 2;
685 break;
686 case 16000:
687 sample_rate = 3;
688 break;
689 case 22050:
690 sample_rate = 4;
691 break;
692 case 24000:
693 sample_rate = 5;
694 break;
695 case 32000:
696 sample_rate = 6;
697 break;
698 case 44100:
699 sample_rate = 7;
700 break;
701 case 48000:
702 sample_rate = 8;
703 break;
704 case 64000:
705 sample_rate = 9;
706 break;
707 case 88200:
708 sample_rate = 10;
709 break;
710 case 96000:
711 sample_rate = 11;
712 break;
713 case 128000:
714 sample_rate = 12;
715 break;
716 case 176400:
717 sample_rate = 13;
718 break;
719 case 192000:
720 sample_rate = 14;
721 break;
722 default:
723 sample_rate = 15;
724 }
725
726 format |= (1 << PROBE_SHIFT_FMT_TYPE) & PROBE_MASK_FMT_TYPE;
727 format |= (sample_rate << PROBE_SHIFT_SAMPLE_RATE) & PROBE_MASK_SAMPLE_RATE;
728 format |= ((channels - 1) << PROBE_SHIFT_NB_CHANNELS) & PROBE_MASK_NB_CHANNELS;
729 format |= ((valid_bytes - 1) << PROBE_SHIFT_SAMPLE_SIZE) & PROBE_MASK_SAMPLE_SIZE;
730 format |= ((container_bytes - 1) << PROBE_SHIFT_CONTAINER_SIZE) & PROBE_MASK_CONTAINER_SIZE;
731 format |= (float_fmt << PROBE_SHIFT_SAMPLE_FMT) & PROBE_MASK_SAMPLE_FMT;
732 format |= (1 << PROBE_SHIFT_INTERLEAVING_ST) & PROBE_MASK_INTERLEAVING_ST;
733
734 return format;
735 }
736
737 /**
738 * \brief General extraction probe callback, called from buffer produce.
739 * It will search for probe point connected to this buffer.
740 * Extraction probe: generate format, header and copy data to probe buffer.
741 * Injection probe: find corresponding DMA, check avail data, copy data,
742 * update pointers and request more data from host if needed.
743 * \param[in] arg pointer (not used).
744 * \param[in] type of notify.
745 * \param[in] data pointer.
746 */
probe_cb_produce(void * arg,enum notify_id type,void * data)747 static void probe_cb_produce(void *arg, enum notify_id type, void *data)
748 {
749 struct probe_pdata *_probe = probe_get();
750 struct buffer_cb_transact *cb_data = data;
751 struct comp_buffer *buffer = cb_data->buffer;
752 struct probe_dma_ext *dma;
753 uint32_t buffer_id;
754 uint32_t head, tail;
755 uint32_t free_bytes = 0;
756 int32_t copy_bytes = 0;
757 int ret;
758 uint32_t i, j;
759 uint32_t format;
760
761 buffer_id = buffer->id;
762
763 /* search for probe point connected to this buffer */
764 for (i = 0; i < CONFIG_PROBE_POINTS_MAX; i++)
765 if (_probe->probe_points[i].buffer_id == buffer_id)
766 break;
767
768 if (i == CONFIG_PROBE_POINTS_MAX) {
769 tr_err(&pr_tr, "probe_cb_produce(): probe not found for buffer id: %d",
770 buffer_id);
771 return;
772 }
773
774 if (_probe->probe_points[i].purpose == PROBE_PURPOSE_EXTRACTION) {
775 format = probe_gen_format(buffer->stream.frame_fmt,
776 buffer->stream.rate,
777 buffer->stream.channels);
778 ret = probe_gen_header(buffer,
779 cb_data->transaction_amount,
780 format);
781 if (ret < 0)
782 goto err;
783
784 /* check if transaction amount exceeds component buffer end addr */
785 /* if yes: divide copying into two stages, head and tail */
786 if ((char *)cb_data->transaction_begin_address +
787 cb_data->transaction_amount > (char *)buffer->stream.end_addr) {
788 head = (uintptr_t)buffer->stream.end_addr -
789 (uintptr_t)cb_data->transaction_begin_address;
790 tail = (uintptr_t)cb_data->transaction_amount - head;
791 ret = copy_to_pbuffer(&_probe->ext_dma.dmapb,
792 cb_data->transaction_begin_address,
793 head);
794 if (ret < 0)
795 goto err;
796
797 ret = copy_to_pbuffer(&_probe->ext_dma.dmapb,
798 buffer->stream.addr, tail);
799 if (ret < 0)
800 goto err;
801 } else {
802 ret = copy_to_pbuffer(&_probe->ext_dma.dmapb,
803 cb_data->transaction_begin_address,
804 cb_data->transaction_amount);
805 if (ret < 0)
806 goto err;
807 }
808 /* check if more than 75% of buffer size is already used */
809 if (_probe->ext_dma.dmapb.size - _probe->ext_dma.dmapb.avail <
810 _probe->ext_dma.dmapb.size >> 2)
811 probe_task(NULL);
812 } else {
813 /* search for DMA used by this probe point */
814 for (j = 0; j < CONFIG_PROBE_DMA_MAX; j++) {
815 if (_probe->inject_dma[j].stream_tag !=
816 PROBE_DMA_INVALID &&
817 _probe->inject_dma[j].stream_tag ==
818 _probe->probe_points[i].stream_tag) {
819 break;
820 }
821 }
822 if (j == CONFIG_PROBE_DMA_MAX) {
823 tr_err(&pr_tr, "probe_cb_produce(): dma not found");
824 return;
825 }
826 dma = &_probe->inject_dma[j];
827 /* get avail data info */
828 ret = dma_get_data_size(dma->dc.chan,
829 &dma->dmapb.avail,
830 &free_bytes);
831 if (ret < 0) {
832 tr_err(&pr_tr, "probe_cb_produce(): dma_get_data_size() failed, ret = %u",
833 ret);
834 goto err;
835 }
836
837 /* check if transaction amount exceeds component buffer end addr */
838 /* if yes: divide copying into two stages, head and tail */
839 if ((char *)cb_data->transaction_begin_address +
840 cb_data->transaction_amount > (char *)cb_data->buffer->stream.end_addr) {
841 head = (char *)cb_data->buffer->stream.end_addr -
842 (char *)cb_data->transaction_begin_address;
843 tail = cb_data->transaction_amount - head;
844
845 ret = copy_from_pbuffer(&dma->dmapb,
846 cb_data->transaction_begin_address, head);
847 if (ret < 0)
848 goto err;
849
850 ret = copy_from_pbuffer(&dma->dmapb,
851 cb_data->buffer->stream.addr, tail);
852 if (ret < 0)
853 goto err;
854 } else {
855 ret = copy_from_pbuffer(&dma->dmapb,
856 cb_data->transaction_begin_address,
857 cb_data->transaction_amount);
858 if (ret < 0)
859 goto err;
860 }
861
862 /* calc how many data can be requested */
863 copy_bytes = dma->dmapb.r_ptr - dma->dmapb.w_ptr;
864 if (copy_bytes < 0)
865 copy_bytes += dma->dmapb.size;
866
867 /* align down to request at least 32 */
868 copy_bytes = ALIGN_DOWN(copy_bytes, 32);
869
870 /* check if copy_bytes is still valid for dma copy */
871 if (copy_bytes > 0) {
872 ret = dma_copy_to_host_nowait(&dma->dc,
873 &dma->config, 0,
874 (void *)dma->dmapb.r_ptr,
875 copy_bytes);
876 if (ret < 0)
877 goto err;
878
879 /* update pointers */
880 dma->dmapb.w_ptr = dma->dmapb.w_ptr + copy_bytes;
881 if (dma->dmapb.w_ptr > dma->dmapb.end_addr)
882 dma->dmapb.w_ptr = dma->dmapb.w_ptr - dma->dmapb.size;
883 }
884 }
885 return;
886 err:
887 tr_err(&pr_tr, "probe_cb_produce(): failed to generate probe data");
888 }
889
890 /**
891 * \brief Callback for buffer free, it will remove probe point.
892 * \param[in] arg pointer (not used).
893 * \param[in] type of notify.
894 * \param[in] data pointer.
895 */
probe_cb_free(void * arg,enum notify_id type,void * data)896 static void probe_cb_free(void *arg, enum notify_id type, void *data)
897 {
898 struct buffer_cb_free *cb_data = data;
899 uint32_t buffer_id = cb_data->buffer->id;
900 int ret;
901
902 tr_dbg(&pr_tr, "probe_cb_free() buffer_id = %u", buffer_id);
903
904 ret = probe_point_remove(1, &buffer_id);
905 if (ret < 0)
906 tr_err(&pr_tr, "probe_cb_free(): probe_point_remove() failed");
907 }
908
probe_point_add(uint32_t count,struct probe_point * probe)909 int probe_point_add(uint32_t count, struct probe_point *probe)
910 {
911 struct probe_pdata *_probe = probe_get();
912 uint32_t i;
913 uint32_t j;
914 uint32_t buffer_id;
915 uint32_t first_free;
916 uint32_t dma_found;
917 struct ipc_comp_dev *dev;
918
919 tr_dbg(&pr_tr, "probe_point_add() count = %u", count);
920
921 if (!_probe) {
922 tr_err(&pr_tr, "probe_point_add(): Not initialized.");
923
924 return -EINVAL;
925 }
926
927 /* add all probe points if they are corresponding to valid component and DMA */
928 for (i = 0; i < count; i++) {
929 tr_dbg(&pr_tr, "\tprobe[%u] buffer_id = %u, purpose = %u, stream_tag = %u",
930 i, probe[i].buffer_id, probe[i].purpose,
931 probe[i].stream_tag);
932
933 if (probe[i].purpose != PROBE_PURPOSE_EXTRACTION &&
934 probe[i].purpose != PROBE_PURPOSE_INJECTION) {
935 tr_err(&pr_tr, "probe_point_add() error: invalid purpose %d",
936 probe[i].purpose);
937
938 return -EINVAL;
939 }
940
941 if (probe[i].purpose == PROBE_PURPOSE_EXTRACTION &&
942 _probe->ext_dma.stream_tag == PROBE_DMA_INVALID) {
943 tr_err(&pr_tr, "probe_point_add(): Setting probe for extraction, while extraction DMA not enabled.");
944
945 return -EINVAL;
946 }
947
948 /* check if buffer exists */
949 dev = ipc_get_comp_by_id(ipc_get(), probe[i].buffer_id);
950 if (!dev) {
951 tr_err(&pr_tr, "probe_point_add(): No device with ID %u found.",
952 probe[i].buffer_id);
953
954 return -EINVAL;
955 }
956
957 if (dev->type != COMP_TYPE_BUFFER) {
958 tr_err(&pr_tr, "probe_point_add(): Device ID %u is not a buffer.",
959 probe[i].buffer_id);
960
961 return -EINVAL;
962 }
963
964 first_free = CONFIG_PROBE_POINTS_MAX;
965
966 /* search for first free probe slot */
967 for (j = 0; j < CONFIG_PROBE_POINTS_MAX; j++) {
968 if (_probe->probe_points[j].stream_tag ==
969 PROBE_POINT_INVALID) {
970 if (first_free == CONFIG_PROBE_POINTS_MAX)
971 first_free = j;
972
973 continue;
974 }
975 /* and check if probe is already attached */
976 buffer_id = _probe->probe_points[j].buffer_id;
977 if (buffer_id == probe[i].buffer_id) {
978 if (_probe->probe_points[j].purpose ==
979 probe[i].purpose) {
980 tr_err(&pr_tr, "probe_point_add(): Probe already attached to buffer %u with purpose %u",
981 buffer_id,
982 probe[i].purpose);
983
984 return -EINVAL;
985 }
986 }
987 }
988
989 if (first_free == CONFIG_PROBE_POINTS_MAX) {
990 tr_err(&pr_tr, "probe_point_add(): Maximum number of probe points connected aleady: "
991 META_QUOTE(CONFIG_PROBE_POINTS_MAX));
992
993 return -EINVAL;
994 }
995
996 /* if connecting injection probe, check for associated DMA */
997 if (probe[i].purpose == PROBE_PURPOSE_INJECTION) {
998 dma_found = 0;
999
1000 for (j = 0; j < CONFIG_PROBE_DMA_MAX; j++) {
1001 if (_probe->inject_dma[j].stream_tag !=
1002 PROBE_DMA_INVALID &&
1003 _probe->inject_dma[j].stream_tag ==
1004 probe[i].stream_tag) {
1005 dma_found = 1;
1006 break;
1007 }
1008 }
1009
1010 if (!dma_found) {
1011 tr_err(&pr_tr, "probe_point_add(): No DMA with stream tag %u found for injection.",
1012 probe[i].stream_tag);
1013
1014 return -EINVAL;
1015 }
1016 if (dma_start(_probe->inject_dma[j].dc.chan) < 0) {
1017 tr_err(&pr_tr, "probe_point_add(): failed to start dma");
1018
1019 return -EBUSY;
1020 }
1021 } else if (probe[i].purpose == PROBE_PURPOSE_EXTRACTION) {
1022 for (j = 0; j < CONFIG_PROBE_POINTS_MAX; j++) {
1023 if (_probe->probe_points[j].stream_tag != PROBE_DMA_INVALID &&
1024 _probe->probe_points[j].purpose == PROBE_PURPOSE_EXTRACTION)
1025 break;
1026 }
1027 if (j == CONFIG_PROBE_POINTS_MAX) {
1028 tr_dbg(&pr_tr, "probe_point_add(): start probe task");
1029 schedule_task(&_probe->dmap_work, 1000, 1000);
1030 }
1031 /* ignore probe stream tag for extraction probes */
1032 probe[i].stream_tag = _probe->ext_dma.stream_tag;
1033 }
1034
1035 /* probe point valid, save it */
1036 _probe->probe_points[first_free].buffer_id = probe[i].buffer_id;
1037 _probe->probe_points[first_free].purpose = probe[i].purpose;
1038 _probe->probe_points[first_free].stream_tag =
1039 probe[i].stream_tag;
1040
1041 notifier_register(_probe, dev->cb, NOTIFIER_ID_BUFFER_PRODUCE,
1042 &probe_cb_produce, 0);
1043 notifier_register(_probe, dev->cb, NOTIFIER_ID_BUFFER_FREE,
1044 &probe_cb_free, 0);
1045 }
1046
1047 return 0;
1048 }
1049
probe_point_info(struct sof_ipc_probe_info_params * data,uint32_t max_size)1050 int probe_point_info(struct sof_ipc_probe_info_params *data, uint32_t max_size)
1051 {
1052 struct probe_pdata *_probe = probe_get();
1053 uint32_t i = 0;
1054 uint32_t j = 0;
1055
1056 tr_dbg(&pr_tr, "probe_point_info()");
1057
1058 if (!_probe) {
1059 tr_err(&pr_tr, "probe_point_info(): Not initialized.");
1060
1061 return -EINVAL;
1062 }
1063
1064 data->rhdr.hdr.size = sizeof(*data);
1065 /* search for all probe points to send them in reply */
1066 while (i < CONFIG_PROBE_POINTS_MAX &&
1067 data->rhdr.hdr.size + sizeof(struct probe_point) < max_size) {
1068 if (_probe->probe_points[i].stream_tag != PROBE_POINT_INVALID) {
1069 data->probe_point[j].buffer_id =
1070 _probe->probe_points[i].buffer_id;
1071 data->probe_point[j].purpose =
1072 _probe->probe_points[i].purpose;
1073 data->probe_point[j].stream_tag =
1074 _probe->probe_points[i].stream_tag;
1075 j++;
1076 data->rhdr.hdr.size += sizeof(struct probe_point);
1077 }
1078
1079 i++;
1080 }
1081
1082 data->num_elems = j;
1083
1084 return 1;
1085 }
1086
probe_point_remove(uint32_t count,uint32_t * buffer_id)1087 int probe_point_remove(uint32_t count, uint32_t *buffer_id)
1088 {
1089 struct probe_pdata *_probe = probe_get();
1090 struct ipc_comp_dev *dev;
1091 uint32_t i;
1092 uint32_t j;
1093
1094 tr_dbg(&pr_tr, "probe_point_remove() count = %u", count);
1095
1096 if (!_probe) {
1097 tr_err(&pr_tr, "probe_point_remove(): Not initialized.");
1098 return -EINVAL;
1099 }
1100 /* remove each requested probe point */
1101 for (i = 0; i < count; i++) {
1102 tr_dbg(&pr_tr, "\tbuffer_id[%u] = %u", i, buffer_id[i]);
1103
1104 for (j = 0; j < CONFIG_PROBE_POINTS_MAX; j++) {
1105 if (_probe->probe_points[j].stream_tag != PROBE_POINT_INVALID &&
1106 _probe->probe_points[j].buffer_id == buffer_id[i]) {
1107 dev = ipc_get_comp_by_id(ipc_get(), buffer_id[i]);
1108 if (dev) {
1109 notifier_unregister(_probe, dev->cb,
1110 NOTIFIER_ID_BUFFER_PRODUCE);
1111 notifier_unregister(_probe, dev->cb,
1112 NOTIFIER_ID_BUFFER_FREE);
1113 }
1114
1115 _probe->probe_points[j].stream_tag =
1116 PROBE_POINT_INVALID;
1117 }
1118 }
1119 }
1120 for (j = 0; j < CONFIG_PROBE_POINTS_MAX; j++) {
1121 if (_probe->probe_points[j].stream_tag != PROBE_DMA_INVALID &&
1122 _probe->probe_points[j].purpose == PROBE_PURPOSE_EXTRACTION)
1123 break;
1124 }
1125 if (j == CONFIG_PROBE_POINTS_MAX) {
1126 tr_dbg(&pr_tr, "probe_point_remove(): cancel probe task");
1127 schedule_task_cancel(&_probe->dmap_work);
1128 }
1129
1130 return 0;
1131 }
1132
