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