ipu7-isys-queue.c

  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
 32static 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
 50static 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
 62static 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
 85static 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 */
110void 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 */
162static 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 */
203static 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
243static 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)
260void 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. */
284static 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
335out_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
348static 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
433out:
434	mutex_unlock(&stream->mutex);
435}
436
437static 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
479static 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
520static 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
527static 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
594out:
595	mutex_unlock(&stream->mutex);
596
597	return 0;
598
599out_isys_fw_close:
600	ipu7_isys_fw_close(av->isys);
601
602out_stream_start:
603	list_del(&aq->node);
604	stream->nr_streaming--;
605	mutex_unlock(&stream->mutex);
606
607out_pipeline_stop:
608	ipu7_isys_stream_cleanup(av);
609
610out_return_buffers:
611	return_buffers(aq, VB2_BUF_STATE_QUEUED);
612
613	return ret;
614}
615
616static 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
641static unsigned int
642get_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
671static 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
686static 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
710static 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
726void 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
789static 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
799int 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}