ipu6-isys-queue.c

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