1// SPDX-License-Identifier: GPL-2.0-only
2/*
3 * Copyright (C) 2013--2024 Intel Corporation
4 */
5#include <linux/atomic.h>
6#include <linux/bug.h>
7#include <linux/device.h>
8#include <linux/list.h>
9#include <linux/lockdep.h>
10#include <linux/mutex.h>
11#include <linux/spinlock.h>
12#include <linux/types.h>
13
14#include <media/media-entity.h>
15#include <media/v4l2-subdev.h>
16#include <media/videobuf2-dma-sg.h>
17#include <media/videobuf2-v4l2.h>
18
19#include "ipu6-bus.h"
20#include "ipu6-dma.h"
21#include "ipu6-fw-isys.h"
22#include "ipu6-isys.h"
23#include "ipu6-isys-video.h"
24
25static int ipu6_isys_buf_init(struct vb2_buffer *vb)
26{
27 struct ipu6_isys *isys = vb2_get_drv_priv(q: vb->vb2_queue);
28 struct sg_table *sg = vb2_dma_sg_plane_desc(vb, plane_no: 0);
29 struct vb2_v4l2_buffer *vvb = to_vb2_v4l2_buffer(vb);
30 struct ipu6_isys_video_buffer *ivb =
31 vb2_buffer_to_ipu6_isys_video_buffer(vvb);
32 int ret;
33
34 ret = ipu6_dma_map_sgtable(sys: isys->adev, sgt: sg, dir: DMA_TO_DEVICE, attrs: 0);
35 if (ret)
36 return ret;
37
38 ivb->dma_addr = sg_dma_address(sg->sgl);
39
40 return 0;
41}
42
43static void ipu6_isys_buf_cleanup(struct vb2_buffer *vb)
44{
45 struct ipu6_isys *isys = vb2_get_drv_priv(q: vb->vb2_queue);
46 struct sg_table *sg = vb2_dma_sg_plane_desc(vb, plane_no: 0);
47 struct vb2_v4l2_buffer *vvb = to_vb2_v4l2_buffer(vb);
48 struct ipu6_isys_video_buffer *ivb =
49 vb2_buffer_to_ipu6_isys_video_buffer(vvb);
50
51 ivb->dma_addr = 0;
52 ipu6_dma_unmap_sgtable(sys: isys->adev, sgt: sg, dir: DMA_TO_DEVICE, attrs: 0);
53}
54
55static int ipu6_isys_queue_setup(struct vb2_queue *q, unsigned int *num_buffers,
56 unsigned int *num_planes, unsigned int sizes[],
57 struct device *alloc_devs[])
58{
59 struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(q);
60 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
61 struct device *dev = &av->isys->adev->auxdev.dev;
62 u32 size = ipu6_isys_get_data_size(av);
63
64 /* num_planes == 0: we're being called through VIDIOC_REQBUFS */
65 if (!*num_planes) {
66 sizes[0] = size;
67 } else if (sizes[0] < size) {
68 dev_dbg(dev, "%s: queue setup: size %u < %u\n",
69 av->vdev.name, sizes[0], size);
70 return -EINVAL;
71 }
72
73 *num_planes = 1;
74
75 return 0;
76}
77
78static int ipu6_isys_buf_prepare(struct vb2_buffer *vb)
79{
80 struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(vb->vb2_queue);
81 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
82 struct device *dev = &av->isys->adev->auxdev.dev;
83 u32 bytesperline = ipu6_isys_get_bytes_per_line(av);
84 u32 height = ipu6_isys_get_frame_height(av);
85 u32 size = ipu6_isys_get_data_size(av);
86
87 dev_dbg(dev, "buffer: %s: configured size %u, buffer size %lu\n",
88 av->vdev.name, size, vb2_plane_size(vb, 0));
89
90 if (size > vb2_plane_size(vb, plane_no: 0))
91 return -EINVAL;
92
93 vb2_set_plane_payload(vb, plane_no: 0, size: bytesperline * height);
94
95 return 0;
96}
97
98/*
99 * Queue a buffer list back to incoming or active queues. The buffers
100 * are removed from the buffer list.
101 */
102void ipu6_isys_buffer_list_queue(struct ipu6_isys_buffer_list *bl,
103 unsigned long op_flags,
104 enum vb2_buffer_state state)
105{
106 struct ipu6_isys_buffer *ib, *ib_safe;
107 unsigned long flags;
108 bool first = true;
109
110 if (!bl)
111 return;
112
113 WARN_ON_ONCE(!bl->nbufs);
114 WARN_ON_ONCE(op_flags & IPU6_ISYS_BUFFER_LIST_FL_ACTIVE &&
115 op_flags & IPU6_ISYS_BUFFER_LIST_FL_INCOMING);
116
117 list_for_each_entry_safe(ib, ib_safe, &bl->head, head) {
118 struct ipu6_isys_video *av;
119 struct vb2_buffer *vb = ipu6_isys_buffer_to_vb2_buffer(ib);
120 struct ipu6_isys_queue *aq =
121 vb2_queue_to_isys_queue(vb->vb2_queue);
122 struct device *dev;
123
124 av = ipu6_isys_queue_to_video(aq);
125 dev = &av->isys->adev->auxdev.dev;
126 spin_lock_irqsave(&aq->lock, flags);
127 list_del(entry: &ib->head);
128 if (op_flags & IPU6_ISYS_BUFFER_LIST_FL_ACTIVE)
129 list_add(new: &ib->head, head: &aq->active);
130 else if (op_flags & IPU6_ISYS_BUFFER_LIST_FL_INCOMING)
131 list_add_tail(new: &ib->head, head: &aq->incoming);
132 spin_unlock_irqrestore(lock: &aq->lock, flags);
133
134 if (op_flags & IPU6_ISYS_BUFFER_LIST_FL_SET_STATE)
135 vb2_buffer_done(vb, state);
136
137 if (first) {
138 dev_dbg(dev,
139 "queue buf list %p flags %lx, s %d, %d bufs\n",
140 bl, op_flags, state, bl->nbufs);
141 first = false;
142 }
143
144 bl->nbufs--;
145 }
146
147 WARN_ON(bl->nbufs);
148}
149
150/*
151 * flush_firmware_streamon_fail() - Flush in cases where requests may
152 * have been queued to firmware and the *firmware streamon fails for a
153 * reason or another.
154 */
155static void flush_firmware_streamon_fail(struct ipu6_isys_stream *stream)
156{
157 struct device *dev = &stream->isys->adev->auxdev.dev;
158 struct ipu6_isys_queue *aq;
159 unsigned long flags;
160
161 lockdep_assert_held(&stream->mutex);
162
163 list_for_each_entry(aq, &stream->queues, node) {
164 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
165 struct ipu6_isys_buffer *ib, *ib_safe;
166
167 spin_lock_irqsave(&aq->lock, flags);
168 list_for_each_entry_safe(ib, ib_safe, &aq->active, head) {
169 struct vb2_buffer *vb =
170 ipu6_isys_buffer_to_vb2_buffer(ib);
171
172 list_del(entry: &ib->head);
173 if (av->streaming) {
174 dev_dbg(dev,
175 "%s: queue buffer %u back to incoming\n",
176 av->vdev.name, vb->index);
177 /* Queue already streaming, return to driver. */
178 list_add(new: &ib->head, head: &aq->incoming);
179 continue;
180 }
181 /* Queue not yet streaming, return to user. */
182 dev_dbg(dev, "%s: return %u back to videobuf2\n",
183 av->vdev.name, vb->index);
184 vb2_buffer_done(ipu6_isys_buffer_to_vb2_buffer(ib),
185 state: VB2_BUF_STATE_QUEUED);
186 }
187 spin_unlock_irqrestore(lock: &aq->lock, flags);
188 }
189}
190
191/*
192 * Attempt obtaining a buffer list from the incoming queues, a list of buffers
193 * that contains one entry from each video buffer queue. If a buffer can't be
194 * obtained from every queue, the buffers are returned back to the queue.
195 */
196static int buffer_list_get(struct ipu6_isys_stream *stream,
197 struct ipu6_isys_buffer_list *bl)
198{
199 struct device *dev = &stream->isys->adev->auxdev.dev;
200 struct ipu6_isys_queue *aq;
201 unsigned long flags;
202 unsigned long buf_flag = IPU6_ISYS_BUFFER_LIST_FL_INCOMING;
203
204 bl->nbufs = 0;
205 INIT_LIST_HEAD(list: &bl->head);
206
207 list_for_each_entry(aq, &stream->queues, node) {
208 struct ipu6_isys_buffer *ib;
209
210 spin_lock_irqsave(&aq->lock, flags);
211 if (list_empty(head: &aq->incoming)) {
212 spin_unlock_irqrestore(lock: &aq->lock, flags);
213 if (!list_empty(head: &bl->head))
214 ipu6_isys_buffer_list_queue(bl, op_flags: buf_flag, state: 0);
215 return -ENODATA;
216 }
217
218 ib = list_last_entry(&aq->incoming,
219 struct ipu6_isys_buffer, head);
220
221 dev_dbg(dev, "buffer: %s: buffer %u\n",
222 ipu6_isys_queue_to_video(aq)->vdev.name,
223 ipu6_isys_buffer_to_vb2_buffer(ib)->index);
224 list_del(entry: &ib->head);
225 list_add(new: &ib->head, head: &bl->head);
226 spin_unlock_irqrestore(lock: &aq->lock, flags);
227
228 bl->nbufs++;
229 }
230
231 dev_dbg(dev, "get buffer list %p, %u buffers\n", bl, bl->nbufs);
232
233 return 0;
234}
235
236static void
237ipu6_isys_buf_to_fw_frame_buf_pin(struct vb2_buffer *vb,
238 struct ipu6_fw_isys_frame_buff_set_abi *set)
239{
240 struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(vb->vb2_queue);
241 struct vb2_v4l2_buffer *vvb = to_vb2_v4l2_buffer(vb);
242 struct ipu6_isys_video_buffer *ivb =
243 vb2_buffer_to_ipu6_isys_video_buffer(vvb);
244
245 set->output_pins[aq->fw_output].addr = ivb->dma_addr;
246 set->output_pins[aq->fw_output].out_buf_id = vb->index + 1;
247}
248
249/*
250 * Convert a buffer list to a isys fw ABI framebuffer set. The
251 * buffer list is not modified.
252 */
253#define IPU6_ISYS_FRAME_NUM_THRESHOLD (30)
254void
255ipu6_isys_buf_to_fw_frame_buf(struct ipu6_fw_isys_frame_buff_set_abi *set,
256 struct ipu6_isys_stream *stream,
257 struct ipu6_isys_buffer_list *bl)
258{
259 struct ipu6_isys_buffer *ib;
260
261 WARN_ON(!bl->nbufs);
262
263 set->send_irq_sof = 1;
264 set->send_resp_sof = 1;
265 set->send_irq_eof = 0;
266 set->send_resp_eof = 0;
267
268 if (stream->streaming)
269 set->send_irq_capture_ack = 0;
270 else
271 set->send_irq_capture_ack = 1;
272 set->send_irq_capture_done = 0;
273
274 set->send_resp_capture_ack = 1;
275 set->send_resp_capture_done = 1;
276 if (atomic_read(v: &stream->sequence) >= IPU6_ISYS_FRAME_NUM_THRESHOLD) {
277 set->send_resp_capture_ack = 0;
278 set->send_resp_capture_done = 0;
279 }
280
281 list_for_each_entry(ib, &bl->head, head) {
282 struct vb2_buffer *vb = ipu6_isys_buffer_to_vb2_buffer(ib);
283
284 ipu6_isys_buf_to_fw_frame_buf_pin(vb, set);
285 }
286}
287
288/* Start streaming for real. The buffer list must be available. */
289static int ipu6_isys_stream_start(struct ipu6_isys_video *av,
290 struct ipu6_isys_buffer_list *bl, bool error)
291{
292 struct ipu6_isys_stream *stream = av->stream;
293 struct device *dev = &stream->isys->adev->auxdev.dev;
294 struct ipu6_isys_buffer_list __bl;
295 int ret;
296
297 mutex_lock(&stream->isys->stream_mutex);
298 ret = ipu6_isys_video_set_streaming(av, state: 1, bl);
299 mutex_unlock(lock: &stream->isys->stream_mutex);
300 if (ret)
301 goto out_requeue;
302
303 stream->streaming = 1;
304
305 bl = &__bl;
306
307 do {
308 struct ipu6_fw_isys_frame_buff_set_abi *buf = NULL;
309 struct isys_fw_msgs *msg;
310 u16 send_type = IPU6_FW_ISYS_SEND_TYPE_STREAM_CAPTURE;
311
312 ret = buffer_list_get(stream, bl);
313 if (ret < 0)
314 break;
315
316 msg = ipu6_get_fw_msg_buf(stream);
317 if (!msg)
318 return -ENOMEM;
319
320 buf = &msg->fw_msg.frame;
321 ipu6_isys_buf_to_fw_frame_buf(set: buf, stream, bl);
322 ipu6_fw_isys_dump_frame_buff_set(dev, buf,
323 outputs: stream->nr_output_pins);
324 ipu6_isys_buffer_list_queue(bl, IPU6_ISYS_BUFFER_LIST_FL_ACTIVE,
325 state: 0);
326 ret = ipu6_fw_isys_complex_cmd(isys: stream->isys,
327 stream_handle: stream->stream_handle, cpu_mapped_buf: buf,
328 dma_mapped_buf: msg->dma_addr, size: sizeof(*buf),
329 send_type);
330 } while (!WARN_ON(ret));
331
332 return 0;
333
334out_requeue:
335 if (bl && bl->nbufs)
336 ipu6_isys_buffer_list_queue(bl,
337 IPU6_ISYS_BUFFER_LIST_FL_INCOMING |
338 (error ?
339 IPU6_ISYS_BUFFER_LIST_FL_SET_STATE :
340 0), state: error ? VB2_BUF_STATE_ERROR :
341 VB2_BUF_STATE_QUEUED);
342 flush_firmware_streamon_fail(stream);
343
344 return ret;
345}
346
347static void buf_queue(struct vb2_buffer *vb)
348{
349 struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(vb->vb2_queue);
350 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
351 struct vb2_v4l2_buffer *vvb = to_vb2_v4l2_buffer(vb);
352 struct ipu6_isys_video_buffer *ivb =
353 vb2_buffer_to_ipu6_isys_video_buffer(vvb);
354 struct ipu6_isys_buffer *ib = &ivb->ib;
355 struct device *dev = &av->isys->adev->auxdev.dev;
356 struct media_pipeline *media_pipe =
357 media_entity_pipeline(entity: &av->vdev.entity);
358 struct ipu6_fw_isys_frame_buff_set_abi *buf = NULL;
359 struct ipu6_isys_stream *stream = av->stream;
360 struct ipu6_isys_buffer_list bl;
361 struct isys_fw_msgs *msg;
362 unsigned long flags;
363 dma_addr_t dma;
364 int ret;
365
366 dev_dbg(dev, "queue buffer %u for %s\n", vb->index, av->vdev.name);
367
368 dma = ivb->dma_addr;
369 dev_dbg(dev, "iova: iova %pad\n", &dma);
370
371 spin_lock_irqsave(&aq->lock, flags);
372 list_add(new: &ib->head, head: &aq->incoming);
373 spin_unlock_irqrestore(lock: &aq->lock, flags);
374
375 if (!media_pipe || !vb->vb2_queue->start_streaming_called) {
376 dev_dbg(dev, "media pipeline is not ready for %s\n",
377 av->vdev.name);
378 return;
379 }
380
381 mutex_lock(&stream->mutex);
382
383 if (stream->nr_streaming != stream->nr_queues) {
384 dev_dbg(dev, "not streaming yet, adding to incoming\n");
385 goto out;
386 }
387
388 /*
389 * We just put one buffer to the incoming list of this queue
390 * (above). Let's see whether all queues in the pipeline would
391 * have a buffer.
392 */
393 ret = buffer_list_get(stream, bl: &bl);
394 if (ret < 0) {
395 dev_dbg(dev, "No buffers available\n");
396 goto out;
397 }
398
399 msg = ipu6_get_fw_msg_buf(stream);
400 if (!msg) {
401 ret = -ENOMEM;
402 goto out;
403 }
404
405 buf = &msg->fw_msg.frame;
406 ipu6_isys_buf_to_fw_frame_buf(set: buf, stream, bl: &bl);
407 ipu6_fw_isys_dump_frame_buff_set(dev, buf, outputs: stream->nr_output_pins);
408
409 if (!stream->streaming) {
410 ret = ipu6_isys_stream_start(av, bl: &bl, error: true);
411 if (ret)
412 dev_err(dev, "stream start failed.\n");
413 goto out;
414 }
415
416 /*
417 * We must queue the buffers in the buffer list to the
418 * appropriate video buffer queues BEFORE passing them to the
419 * firmware since we could get a buffer event back before we
420 * have queued them ourselves to the active queue.
421 */
422 ipu6_isys_buffer_list_queue(bl: &bl, IPU6_ISYS_BUFFER_LIST_FL_ACTIVE, state: 0);
423
424 ret = ipu6_fw_isys_complex_cmd(isys: stream->isys, stream_handle: stream->stream_handle,
425 cpu_mapped_buf: buf, dma_mapped_buf: msg->dma_addr, size: sizeof(*buf),
426 send_type: IPU6_FW_ISYS_SEND_TYPE_STREAM_CAPTURE);
427 if (ret < 0)
428 dev_err(dev, "send stream capture failed\n");
429
430out:
431 mutex_unlock(lock: &stream->mutex);
432}
433
434static int ipu6_isys_link_fmt_validate(struct ipu6_isys_queue *aq)
435{
436 struct v4l2_mbus_framefmt format;
437 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
438 struct device *dev = &av->isys->adev->auxdev.dev;
439 struct media_pad *remote_pad =
440 media_pad_remote_pad_first(pad: av->vdev.entity.pads);
441 struct v4l2_subdev *sd;
442 u32 r_stream, code;
443 int ret;
444
445 if (!remote_pad)
446 return -ENOTCONN;
447
448 sd = media_entity_to_v4l2_subdev(remote_pad->entity);
449 r_stream = ipu6_isys_get_src_stream_by_src_pad(sd, pad: remote_pad->index);
450
451 ret = ipu6_isys_get_stream_pad_fmt(sd, pad: remote_pad->index, stream: r_stream,
452 format: &format);
453
454 if (ret) {
455 dev_dbg(dev, "failed to get %s: pad %d, stream:%d format\n",
456 sd->entity.name, remote_pad->index, r_stream);
457 return ret;
458 }
459
460 if (format.width != ipu6_isys_get_frame_width(av) ||
461 format.height != ipu6_isys_get_frame_height(av)) {
462 dev_dbg(dev, "wrong width or height %ux%u (%ux%u expected)\n",
463 ipu6_isys_get_frame_width(av),
464 ipu6_isys_get_frame_height(av), format.width,
465 format.height);
466 return -EINVAL;
467 }
468
469 code = ipu6_isys_get_isys_format(pixelformat: ipu6_isys_get_format(av), code: 0)->code;
470 if (format.code != code) {
471 dev_dbg(dev, "wrong mbus code 0x%8.8x (0x%8.8x expected)\n",
472 code, format.code);
473 return -EINVAL;
474 }
475
476 return 0;
477}
478
479static void return_buffers(struct ipu6_isys_queue *aq,
480 enum vb2_buffer_state state)
481{
482 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
483 struct ipu6_isys_buffer *ib;
484 bool need_reset = false;
485 unsigned long flags;
486
487 spin_lock_irqsave(&aq->lock, flags);
488 while (!list_empty(head: &aq->incoming)) {
489 struct vb2_buffer *vb;
490
491 ib = list_first_entry(&aq->incoming, struct ipu6_isys_buffer,
492 head);
493 vb = ipu6_isys_buffer_to_vb2_buffer(ib);
494 list_del(entry: &ib->head);
495 spin_unlock_irqrestore(lock: &aq->lock, flags);
496
497 vb2_buffer_done(vb, state);
498
499 spin_lock_irqsave(&aq->lock, flags);
500 }
501
502 /*
503 * Something went wrong (FW crash / HW hang / not all buffers
504 * returned from isys) if there are still buffers queued in active
505 * queue. We have to clean up places a bit.
506 */
507 while (!list_empty(head: &aq->active)) {
508 struct vb2_buffer *vb;
509
510 ib = list_first_entry(&aq->active, struct ipu6_isys_buffer,
511 head);
512 vb = ipu6_isys_buffer_to_vb2_buffer(ib);
513
514 list_del(entry: &ib->head);
515 spin_unlock_irqrestore(lock: &aq->lock, flags);
516
517 vb2_buffer_done(vb, state);
518
519 spin_lock_irqsave(&aq->lock, flags);
520 need_reset = true;
521 }
522
523 spin_unlock_irqrestore(lock: &aq->lock, flags);
524
525 if (need_reset) {
526 mutex_lock(&av->isys->mutex);
527 av->isys->need_reset = true;
528 mutex_unlock(lock: &av->isys->mutex);
529 }
530}
531
532static void ipu6_isys_stream_cleanup(struct ipu6_isys_video *av)
533{
534 video_device_pipeline_stop(vdev: &av->vdev);
535 ipu6_isys_put_stream(stream: av->stream);
536 av->stream = NULL;
537}
538
539static int start_streaming(struct vb2_queue *q, unsigned int count)
540{
541 struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(q);
542 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
543 struct device *dev = &av->isys->adev->auxdev.dev;
544 const struct ipu6_isys_pixelformat *pfmt =
545 ipu6_isys_get_isys_format(pixelformat: ipu6_isys_get_format(av), code: 0);
546 struct ipu6_isys_buffer_list __bl, *bl = NULL;
547 struct ipu6_isys_stream *stream;
548 struct media_entity *source_entity = NULL;
549 int nr_queues, ret;
550
551 dev_dbg(dev, "stream: %s: width %u, height %u, css pixelformat %u\n",
552 av->vdev.name, ipu6_isys_get_frame_width(av),
553 ipu6_isys_get_frame_height(av), pfmt->css_pixelformat);
554
555 ret = ipu6_isys_setup_video(av, source_entity: &source_entity, nr_queues: &nr_queues);
556 if (ret < 0) {
557 dev_dbg(dev, "failed to setup video\n");
558 goto out_return_buffers;
559 }
560
561 ret = ipu6_isys_link_fmt_validate(aq);
562 if (ret) {
563 dev_dbg(dev,
564 "%s: link format validation failed (%d)\n",
565 av->vdev.name, ret);
566 goto out_pipeline_stop;
567 }
568
569 ret = ipu6_isys_fw_open(isys: av->isys);
570 if (ret)
571 goto out_pipeline_stop;
572
573 stream = av->stream;
574 mutex_lock(&stream->mutex);
575 if (!stream->nr_streaming) {
576 ret = ipu6_isys_video_prepare_stream(av, source_entity,
577 nr_queues);
578 if (ret)
579 goto out_fw_close;
580 }
581
582 stream->nr_streaming++;
583 dev_dbg(dev, "queue %u of %u\n", stream->nr_streaming,
584 stream->nr_queues);
585
586 list_add(new: &aq->node, head: &stream->queues);
587 ipu6_isys_configure_stream_watermark(av, state: true);
588 ipu6_isys_update_stream_watermark(av, state: true);
589
590 if (stream->nr_streaming != stream->nr_queues)
591 goto out;
592
593 bl = &__bl;
594 ret = buffer_list_get(stream, bl);
595 if (ret < 0) {
596 dev_warn(dev, "no buffer available, DRIVER BUG?\n");
597 goto out;
598 }
599
600 ret = ipu6_isys_stream_start(av, bl, error: false);
601 if (ret)
602 goto out_stream_start;
603
604out:
605 mutex_unlock(lock: &stream->mutex);
606
607 return 0;
608
609out_stream_start:
610 ipu6_isys_update_stream_watermark(av, state: false);
611 list_del(entry: &aq->node);
612 stream->nr_streaming--;
613
614out_fw_close:
615 mutex_unlock(lock: &stream->mutex);
616 ipu6_isys_fw_close(isys: av->isys);
617
618out_pipeline_stop:
619 ipu6_isys_stream_cleanup(av);
620
621out_return_buffers:
622 return_buffers(aq, state: VB2_BUF_STATE_QUEUED);
623
624 return ret;
625}
626
627static void stop_streaming(struct vb2_queue *q)
628{
629 struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(q);
630 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
631 struct ipu6_isys_stream *stream = av->stream;
632
633 mutex_lock(&stream->mutex);
634
635 ipu6_isys_update_stream_watermark(av, state: false);
636
637 mutex_lock(&av->isys->stream_mutex);
638 if (stream->nr_streaming == stream->nr_queues && stream->streaming)
639 ipu6_isys_video_set_streaming(av, state: 0, NULL);
640 mutex_unlock(lock: &av->isys->stream_mutex);
641
642 stream->nr_streaming--;
643 list_del(entry: &aq->node);
644 stream->streaming = 0;
645 mutex_unlock(lock: &stream->mutex);
646
647 ipu6_isys_stream_cleanup(av);
648
649 return_buffers(aq, state: VB2_BUF_STATE_ERROR);
650
651 ipu6_isys_fw_close(isys: av->isys);
652}
653
654static unsigned int
655get_sof_sequence_by_timestamp(struct ipu6_isys_stream *stream, u64 time)
656{
657 struct ipu6_isys *isys = stream->isys;
658 struct device *dev = &isys->adev->auxdev.dev;
659 unsigned int i;
660
661 /*
662 * The timestamp is invalid as no TSC in some FPGA platform,
663 * so get the sequence from pipeline directly in this case.
664 */
665 if (time == 0)
666 return atomic_read(v: &stream->sequence) - 1;
667
668 for (i = 0; i < IPU6_ISYS_MAX_PARALLEL_SOF; i++)
669 if (time == stream->seq[i].timestamp) {
670 dev_dbg(dev, "sof: using seq nr %u for ts %llu\n",
671 stream->seq[i].sequence, time);
672 return stream->seq[i].sequence;
673 }
674
675 for (i = 0; i < IPU6_ISYS_MAX_PARALLEL_SOF; i++)
676 dev_dbg(dev, "sof: sequence %u, timestamp value %llu\n",
677 stream->seq[i].sequence, stream->seq[i].timestamp);
678
679 return 0;
680}
681
682static u64 get_sof_ns_delta(struct ipu6_isys_video *av, u64 timestamp)
683{
684 struct ipu6_bus_device *adev = av->isys->adev;
685 struct ipu6_device *isp = adev->isp;
686 u64 delta, tsc_now;
687
688 ipu6_buttress_tsc_read(isp, val: &tsc_now);
689 if (!tsc_now)
690 return 0;
691
692 delta = tsc_now - timestamp;
693
694 return ipu6_buttress_tsc_ticks_to_ns(ticks: delta, isp);
695}
696
697static void
698ipu6_isys_buf_calc_sequence_time(struct ipu6_isys_buffer *ib, u64 time)
699{
700 struct vb2_buffer *vb = ipu6_isys_buffer_to_vb2_buffer(ib);
701 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
702 struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(vb->vb2_queue);
703 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
704 struct device *dev = &av->isys->adev->auxdev.dev;
705 struct ipu6_isys_stream *stream = av->stream;
706 u64 ns;
707 u32 sequence;
708
709 ns = ktime_get_ns() - get_sof_ns_delta(av, timestamp: time);
710 sequence = get_sof_sequence_by_timestamp(stream, time);
711
712 vbuf->vb2_buf.timestamp = ns;
713 vbuf->sequence = sequence;
714
715 dev_dbg(dev, "buf: %s: buffer done, CPU-timestamp:%lld, sequence:%d\n",
716 av->vdev.name, ktime_get_ns(), sequence);
717 dev_dbg(dev, "index:%d, vbuf timestamp:%lld\n", vb->index,
718 vbuf->vb2_buf.timestamp);
719}
720
721static void ipu6_isys_queue_buf_done(struct ipu6_isys_buffer *ib)
722{
723 struct vb2_buffer *vb = ipu6_isys_buffer_to_vb2_buffer(ib);
724
725 if (atomic_read(v: &ib->str2mmio_flag)) {
726 vb2_buffer_done(vb, state: VB2_BUF_STATE_ERROR);
727 /*
728 * Operation on buffer is ended with error and will be reported
729 * to the userspace when it is de-queued
730 */
731 atomic_set(v: &ib->str2mmio_flag, i: 0);
732 } else {
733 vb2_buffer_done(vb, state: VB2_BUF_STATE_DONE);
734 }
735}
736
737static void
738ipu6_stream_buf_ready(struct ipu6_isys_stream *stream, u8 pin_id, u32 pin_addr,
739 u64 time, bool error_check)
740{
741 struct ipu6_isys_queue *aq = stream->output_pins_queue[pin_id];
742 struct ipu6_isys *isys = stream->isys;
743 struct device *dev = &isys->adev->auxdev.dev;
744 struct ipu6_isys_buffer *ib;
745 struct vb2_buffer *vb;
746 unsigned long flags;
747 bool first = true;
748 struct vb2_v4l2_buffer *buf;
749
750 spin_lock_irqsave(&aq->lock, flags);
751 if (list_empty(head: &aq->active)) {
752 spin_unlock_irqrestore(lock: &aq->lock, flags);
753 dev_err(dev, "active queue empty\n");
754 return;
755 }
756
757 list_for_each_entry_reverse(ib, &aq->active, head) {
758 struct ipu6_isys_video_buffer *ivb;
759 struct vb2_v4l2_buffer *vvb;
760 dma_addr_t addr;
761
762 vb = ipu6_isys_buffer_to_vb2_buffer(ib);
763 vvb = to_vb2_v4l2_buffer(vb);
764 ivb = vb2_buffer_to_ipu6_isys_video_buffer(vvb);
765 addr = ivb->dma_addr;
766
767 if (pin_addr != addr) {
768 if (first)
769 dev_err(dev, "Unexpected buffer address %pad\n",
770 &addr);
771 first = false;
772 continue;
773 }
774
775 if (error_check) {
776 /*
777 * Check for error message:
778 * 'IPU6_FW_ISYS_ERROR_HW_REPORTED_STR2MMIO'
779 */
780 atomic_set(v: &ib->str2mmio_flag, i: 1);
781 }
782 dev_dbg(dev, "buffer: found buffer %pad\n", &addr);
783
784 buf = to_vb2_v4l2_buffer(vb);
785 buf->field = V4L2_FIELD_NONE;
786
787 list_del(entry: &ib->head);
788 spin_unlock_irqrestore(lock: &aq->lock, flags);
789
790 ipu6_isys_buf_calc_sequence_time(ib, time);
791
792 ipu6_isys_queue_buf_done(ib);
793
794 return;
795 }
796
797 dev_err(dev, "Failed to find a matching video buffer\n");
798
799 spin_unlock_irqrestore(lock: &aq->lock, flags);
800}
801
802void ipu6_isys_queue_buf_ready(struct ipu6_isys_stream *stream,
803 struct ipu6_fw_isys_resp_info_abi *info)
804{
805 u64 time = (u64)info->timestamp[1] << 32 | info->timestamp[0];
806 bool err = info->error_info.error == IPU6_FW_ISYS_ERROR_HW_REPORTED_STR2MMIO;
807
808 ipu6_stream_buf_ready(stream, pin_id: info->pin_id, pin_addr: info->pin.addr, time, error_check: err);
809}
810
811static const struct vb2_ops ipu6_isys_queue_ops = {
812 .queue_setup = ipu6_isys_queue_setup,
813 .buf_init = ipu6_isys_buf_init,
814 .buf_prepare = ipu6_isys_buf_prepare,
815 .buf_cleanup = ipu6_isys_buf_cleanup,
816 .start_streaming = start_streaming,
817 .stop_streaming = stop_streaming,
818 .buf_queue = buf_queue,
819};
820
821int ipu6_isys_queue_init(struct ipu6_isys_queue *aq)
822{
823 struct ipu6_isys *isys = ipu6_isys_queue_to_video(aq)->isys;
824 struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
825 struct ipu6_bus_device *adev = isys->adev;
826 int ret;
827
828 /* no support for userptr */
829 if (!aq->vbq.io_modes)
830 aq->vbq.io_modes = VB2_MMAP | VB2_DMABUF;
831
832 aq->vbq.drv_priv = isys;
833 aq->vbq.ops = &ipu6_isys_queue_ops;
834 aq->vbq.lock = &av->mutex;
835 aq->vbq.mem_ops = &vb2_dma_sg_memops;
836 aq->vbq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
837 aq->vbq.min_queued_buffers = 1;
838 aq->vbq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
839
840 ret = vb2_queue_init(q: &aq->vbq);
841 if (ret)
842 return ret;
843
844 aq->vbq.dev = &adev->isp->pdev->dev;
845 spin_lock_init(&aq->lock);
846 INIT_LIST_HEAD(list: &aq->active);
847 INIT_LIST_HEAD(list: &aq->incoming);
848
849 return 0;
850}
851

source code of linux/drivers/media/pci/intel/ipu6/ipu6-isys-queue.c