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