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