dcmipp-bytecap.c

  1// SPDX-License-Identifier: GPL-2.0
  2/*
  3 * Driver for STM32 Digital Camera Memory Interface Pixel Processor
  4 *
  5 * Copyright (C) STMicroelectronics SA 2023
  6 * Authors: Hugues Fruchet <hugues.fruchet@foss.st.com>
  7 *          Alain Volmat <alain.volmat@foss.st.com>
  8 *          for STMicroelectronics.
  9 */
 10
 11#include <linux/iopoll.h>
 12#include <linux/pm_runtime.h>
 13#include <media/v4l2-ioctl.h>
 14#include <media/v4l2-mc.h>
 15#include <media/videobuf2-core.h>
 16#include <media/videobuf2-dma-contig.h>
 17
 18#include "dcmipp-common.h"
 19
 20#define DCMIPP_PRSR		0x1f8
 21#define DCMIPP_CMIER		0x3f0
 22#define DCMIPP_CMIER_P0FRAMEIE	BIT(9)
 23#define DCMIPP_CMIER_P0VSYNCIE	BIT(10)
 24#define DCMIPP_CMIER_P0OVRIE	BIT(15)
 25#define DCMIPP_CMIER_P0ALL	(DCMIPP_CMIER_P0VSYNCIE |\
 26				 DCMIPP_CMIER_P0FRAMEIE |\
 27				 DCMIPP_CMIER_P0OVRIE)
 28#define DCMIPP_CMSR1		0x3f4
 29#define DCMIPP_CMSR2		0x3f8
 30#define DCMIPP_CMSR2_P0FRAMEF	BIT(9)
 31#define DCMIPP_CMSR2_P0VSYNCF	BIT(10)
 32#define DCMIPP_CMSR2_P0OVRF	BIT(15)
 33#define DCMIPP_CMFCR		0x3fc
 34#define DCMIPP_P0FSCR		0x404
 35#define DCMIPP_P0FSCR_PIPEN	BIT(31)
 36#define DCMIPP_P0FCTCR		0x500
 37#define DCMIPP_P0FCTCR_CPTREQ	BIT(3)
 38#define DCMIPP_P0DCCNTR		0x5b0
 39#define DCMIPP_P0DCLMTR		0x5b4
 40#define DCMIPP_P0DCLMTR_ENABLE	BIT(31)
 41#define DCMIPP_P0DCLMTR_LIMIT_MASK	GENMASK(23, 0)
 42#define DCMIPP_P0PPM0AR1	0x5c4
 43#define DCMIPP_P0SR		0x5f8
 44#define DCMIPP_P0SR_CPTACT	BIT(23)
 45
 46struct dcmipp_bytecap_pix_map {
 47	unsigned int code;
 48	u32 pixelformat;
 49};
 50
 51#define PIXMAP_MBUS_PFMT(mbus, fmt)			\
 52	{						\
 53		.code = MEDIA_BUS_FMT_##mbus,		\
 54		.pixelformat = V4L2_PIX_FMT_##fmt	\
 55	}
 56
 57static const struct dcmipp_bytecap_pix_map dcmipp_bytecap_pix_map_list[] = {
 58	PIXMAP_MBUS_PFMT(RGB565_2X8_LE, RGB565),
 59	PIXMAP_MBUS_PFMT(RGB565_1X16, RGB565),
 60	PIXMAP_MBUS_PFMT(YUYV8_2X8, YUYV),
 61	PIXMAP_MBUS_PFMT(YUYV8_1X16, YUYV),
 62	PIXMAP_MBUS_PFMT(YVYU8_2X8, YVYU),
 63	PIXMAP_MBUS_PFMT(YVYU8_1X16, YVYU),
 64	PIXMAP_MBUS_PFMT(UYVY8_2X8, UYVY),
 65	PIXMAP_MBUS_PFMT(UYVY8_1X16, UYVY),
 66	PIXMAP_MBUS_PFMT(VYUY8_2X8, VYUY),
 67	PIXMAP_MBUS_PFMT(VYUY8_1X16, VYUY),
 68	PIXMAP_MBUS_PFMT(Y8_1X8, GREY),
 69	PIXMAP_MBUS_PFMT(SBGGR8_1X8, SBGGR8),
 70	PIXMAP_MBUS_PFMT(SGBRG8_1X8, SGBRG8),
 71	PIXMAP_MBUS_PFMT(SGRBG8_1X8, SGRBG8),
 72	PIXMAP_MBUS_PFMT(SRGGB8_1X8, SRGGB8),
 73	PIXMAP_MBUS_PFMT(SBGGR10_1X10, SBGGR10),
 74	PIXMAP_MBUS_PFMT(SGBRG10_1X10, SGBRG10),
 75	PIXMAP_MBUS_PFMT(SGRBG10_1X10, SGRBG10),
 76	PIXMAP_MBUS_PFMT(SRGGB10_1X10, SRGGB10),
 77	PIXMAP_MBUS_PFMT(SBGGR12_1X12, SBGGR12),
 78	PIXMAP_MBUS_PFMT(SGBRG12_1X12, SGBRG12),
 79	PIXMAP_MBUS_PFMT(SGRBG12_1X12, SGRBG12),
 80	PIXMAP_MBUS_PFMT(SRGGB12_1X12, SRGGB12),
 81	PIXMAP_MBUS_PFMT(SBGGR14_1X14, SBGGR14),
 82	PIXMAP_MBUS_PFMT(SGBRG14_1X14, SGBRG14),
 83	PIXMAP_MBUS_PFMT(SGRBG14_1X14, SGRBG14),
 84	PIXMAP_MBUS_PFMT(SRGGB14_1X14, SRGGB14),
 85	PIXMAP_MBUS_PFMT(JPEG_1X8, JPEG),
 86};
 87
 88static const struct dcmipp_bytecap_pix_map *
 89dcmipp_bytecap_pix_map_by_pixelformat(u32 pixelformat)
 90{
 91	unsigned int i;
 92
 93	for (i = 0; i < ARRAY_SIZE(dcmipp_bytecap_pix_map_list); i++) {
 94		if (dcmipp_bytecap_pix_map_list[i].pixelformat == pixelformat)
 95			return &dcmipp_bytecap_pix_map_list[i];
 96	}
 97
 98	return NULL;
 99}
100
101struct dcmipp_buf {
102	struct vb2_v4l2_buffer	vb;
103	bool			prepared;
104	dma_addr_t		addr;
105	size_t			size;
106	struct list_head	list;
107};
108
109enum dcmipp_state {
110	DCMIPP_STOPPED = 0,
111	DCMIPP_WAIT_FOR_BUFFER,
112	DCMIPP_RUNNING,
113};
114
115struct dcmipp_bytecap_device {
116	struct dcmipp_ent_device ved;
117	struct video_device vdev;
118	struct device *dev;
119	struct v4l2_pix_format format;
120	struct vb2_queue queue;
121	struct list_head buffers;
122	/*
123	 * Protects concurrent calls of buf queue / irq handler
124	 * and buffer handling related variables / lists
125	 */
126	spinlock_t irqlock;
127	/* mutex used as vdev and queue lock */
128	struct mutex lock;
129	u32 sequence;
130	struct media_pipeline pipe;
131	struct v4l2_subdev *s_subdev;
132	u32 s_subdev_pad_nb;
133
134	enum dcmipp_state state;
135
136	/*
137	 * DCMIPP driver is handling 2 buffers
138	 * active: buffer into which DCMIPP is currently writing into
139	 * next: buffer given to the DCMIPP and which will become
140	 *       automatically active on next VSYNC
141	 */
142	struct dcmipp_buf *active, *next;
143
144	void __iomem *regs;
145
146	u32 cmier;
147	u32 cmsr2;
148
149	struct {
150		u32 errors;
151		u32 limit;
152		u32 overrun;
153		u32 buffers;
154		u32 vsync;
155		u32 frame;
156		u32 it;
157		u32 underrun;
158		u32 nactive;
159	} count;
160};
161
162static const struct v4l2_pix_format fmt_default = {
163	.width = DCMIPP_FMT_WIDTH_DEFAULT,
164	.height = DCMIPP_FMT_HEIGHT_DEFAULT,
165	.pixelformat = V4L2_PIX_FMT_RGB565,
166	.field = V4L2_FIELD_NONE,
167	.bytesperline = DCMIPP_FMT_WIDTH_DEFAULT * 2,
168	.sizeimage = DCMIPP_FMT_WIDTH_DEFAULT * DCMIPP_FMT_HEIGHT_DEFAULT * 2,
169	.colorspace = DCMIPP_COLORSPACE_DEFAULT,
170	.ycbcr_enc = DCMIPP_YCBCR_ENC_DEFAULT,
171	.quantization = DCMIPP_QUANTIZATION_DEFAULT,
172	.xfer_func = DCMIPP_XFER_FUNC_DEFAULT,
173};
174
175static int dcmipp_bytecap_querycap(struct file *file, void *priv,
176				   struct v4l2_capability *cap)
177{
178	strscpy(cap->driver, DCMIPP_PDEV_NAME, sizeof(cap->driver));
179	strscpy(cap->card, KBUILD_MODNAME, sizeof(cap->card));
180
181	return 0;
182}
183
184static int dcmipp_bytecap_g_fmt_vid_cap(struct file *file, void *priv,
185					struct v4l2_format *f)
186{
187	struct dcmipp_bytecap_device *vcap = video_drvdata(file);
188
189	f->fmt.pix = vcap->format;
190
191	return 0;
192}
193
194static int dcmipp_bytecap_try_fmt_vid_cap(struct file *file, void *priv,
195					  struct v4l2_format *f)
196{
197	struct dcmipp_bytecap_device *vcap = video_drvdata(file);
198	struct v4l2_pix_format *format = &f->fmt.pix;
199	const struct dcmipp_bytecap_pix_map *vpix;
200	u32 in_w, in_h;
201
202	/* Don't accept a pixelformat that is not on the table */
203	vpix = dcmipp_bytecap_pix_map_by_pixelformat(format->pixelformat);
204	if (!vpix)
205		format->pixelformat = fmt_default.pixelformat;
206
207	/* Adjust width & height */
208	in_w = format->width;
209	in_h = format->height;
210	v4l_bound_align_image(&format->width, DCMIPP_FRAME_MIN_WIDTH,
211			      DCMIPP_FRAME_MAX_WIDTH, 0, &format->height,
212			      DCMIPP_FRAME_MIN_HEIGHT, DCMIPP_FRAME_MAX_HEIGHT,
213			      0, 0);
214	if (format->width != in_w || format->height != in_h)
215		dev_dbg(vcap->dev, "resolution updated: %dx%d -> %dx%d\n",
216			in_w, in_h, format->width, format->height);
217
218	if (format->pixelformat == V4L2_PIX_FMT_JPEG) {
219		format->bytesperline = format->width;
220		format->sizeimage = format->bytesperline * format->height;
221	} else {
222		v4l2_fill_pixfmt(format, format->pixelformat,
223				 format->width, format->height);
224	}
225
226	if (format->field == V4L2_FIELD_ANY)
227		format->field = fmt_default.field;
228
229	dcmipp_colorimetry_clamp(format);
230
231	return 0;
232}
233
234static int dcmipp_bytecap_s_fmt_vid_cap(struct file *file, void *priv,
235					struct v4l2_format *f)
236{
237	struct dcmipp_bytecap_device *vcap = video_drvdata(file);
238	int ret;
239
240	/* Do not change the format while stream is on */
241	if (vb2_is_busy(&vcap->queue))
242		return -EBUSY;
243
244	ret = dcmipp_bytecap_try_fmt_vid_cap(file, priv, f);
245	if (ret)
246		return ret;
247
248	dev_dbg(vcap->dev, "%s: format update: old:%ux%u (0x%p4cc, %u, %u, %u, %u) new:%ux%d (0x%p4cc, %u, %u, %u, %u)\n",
249		vcap->vdev.name,
250		/* old */
251		vcap->format.width, vcap->format.height,
252		&vcap->format.pixelformat, vcap->format.colorspace,
253		vcap->format.quantization, vcap->format.xfer_func,
254		vcap->format.ycbcr_enc,
255		/* new */
256		f->fmt.pix.width, f->fmt.pix.height,
257		&f->fmt.pix.pixelformat, f->fmt.pix.colorspace,
258		f->fmt.pix.quantization, f->fmt.pix.xfer_func,
259		f->fmt.pix.ycbcr_enc);
260
261	vcap->format = f->fmt.pix;
262
263	return 0;
264}
265
266static int dcmipp_bytecap_enum_fmt_vid_cap(struct file *file, void *priv,
267					   struct v4l2_fmtdesc *f)
268{
269	const struct dcmipp_bytecap_pix_map *vpix;
270	unsigned int index = f->index;
271	unsigned int i, prev_pixelformat = 0;
272
273	/*
274	 * List up all formats (or only ones matching f->mbus_code), taking
275	 * care of removing duplicated entries (due to support of both
276	 * parallel & csi 16 bits formats
277	 */
278	for (i = 0; i < ARRAY_SIZE(dcmipp_bytecap_pix_map_list); i++) {
279		vpix = &dcmipp_bytecap_pix_map_list[i];
280		/* Skip formats not matching requested mbus code */
281		if (f->mbus_code && vpix->code != f->mbus_code)
282			continue;
283
284		/* Skip duplicated pixelformat */
285		if (vpix->pixelformat == prev_pixelformat)
286			continue;
287
288		prev_pixelformat = vpix->pixelformat;
289
290		if (index == 0)
291			break;
292
293		index--;
294	}
295
296	if (i == ARRAY_SIZE(dcmipp_bytecap_pix_map_list))
297		return -EINVAL;
298
299	f->pixelformat = vpix->pixelformat;
300
301	return 0;
302}
303
304static int dcmipp_bytecap_enum_framesizes(struct file *file, void *fh,
305					  struct v4l2_frmsizeenum *fsize)
306{
307	const struct dcmipp_bytecap_pix_map *vpix;
308
309	if (fsize->index)
310		return -EINVAL;
311
312	/* Only accept code in the pix map table */
313	vpix = dcmipp_bytecap_pix_map_by_pixelformat(fsize->pixel_format);
314	if (!vpix)
315		return -EINVAL;
316
317	fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
318	fsize->stepwise.min_width = DCMIPP_FRAME_MIN_WIDTH;
319	fsize->stepwise.max_width = DCMIPP_FRAME_MAX_WIDTH;
320	fsize->stepwise.min_height = DCMIPP_FRAME_MIN_HEIGHT;
321	fsize->stepwise.max_height = DCMIPP_FRAME_MAX_HEIGHT;
322	fsize->stepwise.step_width = 1;
323	fsize->stepwise.step_height = 1;
324
325	return 0;
326}
327
328static const struct v4l2_file_operations dcmipp_bytecap_fops = {
329	.owner		= THIS_MODULE,
330	.open		= v4l2_fh_open,
331	.release	= vb2_fop_release,
332	.read           = vb2_fop_read,
333	.poll		= vb2_fop_poll,
334	.unlocked_ioctl = video_ioctl2,
335	.mmap           = vb2_fop_mmap,
336};
337
338static const struct v4l2_ioctl_ops dcmipp_bytecap_ioctl_ops = {
339	.vidioc_querycap = dcmipp_bytecap_querycap,
340
341	.vidioc_g_fmt_vid_cap = dcmipp_bytecap_g_fmt_vid_cap,
342	.vidioc_s_fmt_vid_cap = dcmipp_bytecap_s_fmt_vid_cap,
343	.vidioc_try_fmt_vid_cap = dcmipp_bytecap_try_fmt_vid_cap,
344	.vidioc_enum_fmt_vid_cap = dcmipp_bytecap_enum_fmt_vid_cap,
345	.vidioc_enum_framesizes = dcmipp_bytecap_enum_framesizes,
346
347	.vidioc_reqbufs = vb2_ioctl_reqbufs,
348	.vidioc_create_bufs = vb2_ioctl_create_bufs,
349	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
350	.vidioc_querybuf = vb2_ioctl_querybuf,
351	.vidioc_qbuf = vb2_ioctl_qbuf,
352	.vidioc_dqbuf = vb2_ioctl_dqbuf,
353	.vidioc_expbuf = vb2_ioctl_expbuf,
354	.vidioc_streamon = vb2_ioctl_streamon,
355	.vidioc_streamoff = vb2_ioctl_streamoff,
356};
357
358static void dcmipp_start_capture(struct dcmipp_bytecap_device *vcap,
359				 struct dcmipp_buf *buf)
360{
361	/* Set buffer address */
362	reg_write(vcap, DCMIPP_P0PPM0AR1, buf->addr);
363
364	/* Set buffer size */
365	reg_write(vcap, DCMIPP_P0DCLMTR, DCMIPP_P0DCLMTR_ENABLE |
366		  ((buf->size / 4) & DCMIPP_P0DCLMTR_LIMIT_MASK));
367
368	/* Capture request */
369	reg_set(vcap, DCMIPP_P0FCTCR, DCMIPP_P0FCTCR_CPTREQ);
370}
371
372static void dcmipp_bytecap_all_buffers_done(struct dcmipp_bytecap_device *vcap,
373					    enum vb2_buffer_state state)
374{
375	struct dcmipp_buf *buf, *node;
376
377	list_for_each_entry_safe(buf, node, &vcap->buffers, list) {
378		list_del_init(&buf->list);
379		vb2_buffer_done(&buf->vb.vb2_buf, state);
380	}
381}
382
383static int dcmipp_bytecap_start_streaming(struct vb2_queue *vq,
384					  unsigned int count)
385{
386	struct dcmipp_bytecap_device *vcap = vb2_get_drv_priv(vq);
387	struct media_entity *entity = &vcap->vdev.entity;
388	struct dcmipp_buf *buf;
389	struct media_pad *pad;
390	int ret;
391
392	vcap->sequence = 0;
393	memset(&vcap->count, 0, sizeof(vcap->count));
394
395	/*
396	 * Get source subdev - since link is IMMUTABLE, pointer is cached
397	 * within the dcmipp_bytecap_device structure
398	 */
399	if (!vcap->s_subdev) {
400		pad = media_pad_remote_pad_first(&vcap->vdev.entity.pads[0]);
401		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
402			return -EINVAL;
403		vcap->s_subdev = media_entity_to_v4l2_subdev(pad->entity);
404		vcap->s_subdev_pad_nb = pad->index;
405	}
406
407	ret = pm_runtime_resume_and_get(vcap->dev);
408	if (ret < 0) {
409		dev_err(vcap->dev, "%s: Failed to start streaming, cannot get sync (%d)\n",
410			__func__, ret);
411		goto err_buffer_done;
412	}
413
414	ret = media_pipeline_start(entity->pads, &vcap->pipe);
415	if (ret) {
416		dev_dbg(vcap->dev, "%s: Failed to start streaming, media pipeline start error (%d)\n",
417			__func__, ret);
418		goto err_pm_put;
419	}
420
421	ret = v4l2_subdev_enable_streams(vcap->s_subdev,
422					 vcap->s_subdev_pad_nb, BIT_ULL(0));
423	if (ret)
424		goto err_media_pipeline_stop;
425
426	spin_lock_irq(&vcap->irqlock);
427
428	/* Enable pipe at the end of programming */
429	reg_set(vcap, DCMIPP_P0FSCR, DCMIPP_P0FSCR_PIPEN);
430
431	/*
432	 * vb2 framework guarantee that we have at least 'min_queued_buffers'
433	 * buffers in the list at this moment
434	 */
435	vcap->next = list_first_entry(&vcap->buffers, typeof(*buf), list);
436	dev_dbg(vcap->dev, "Start with next [%d] %p phy=%pad\n",
437		vcap->next->vb.vb2_buf.index, vcap->next, &vcap->next->addr);
438
439	dcmipp_start_capture(vcap, vcap->next);
440
441	/* Enable interruptions */
442	vcap->cmier |= DCMIPP_CMIER_P0ALL;
443	reg_set(vcap, DCMIPP_CMIER, vcap->cmier);
444
445	vcap->state = DCMIPP_RUNNING;
446
447	spin_unlock_irq(&vcap->irqlock);
448
449	return 0;
450
451err_media_pipeline_stop:
452	media_pipeline_stop(entity->pads);
453err_pm_put:
454	pm_runtime_put(vcap->dev);
455err_buffer_done:
456	spin_lock_irq(&vcap->irqlock);
457	/*
458	 * Return all buffers to vb2 in QUEUED state.
459	 * This will give ownership back to userspace
460	 */
461	dcmipp_bytecap_all_buffers_done(vcap, VB2_BUF_STATE_QUEUED);
462	vcap->active = NULL;
463	spin_unlock_irq(&vcap->irqlock);
464
465	return ret;
466}
467
468static void dcmipp_dump_status(struct dcmipp_bytecap_device *vcap)
469{
470	struct device *dev = vcap->dev;
471
472	dev_dbg(dev, "[DCMIPP_PRSR]  =%#10.8x\n", reg_read(vcap, DCMIPP_PRSR));
473	dev_dbg(dev, "[DCMIPP_P0SR] =%#10.8x\n", reg_read(vcap, DCMIPP_P0SR));
474	dev_dbg(dev, "[DCMIPP_P0DCCNTR]=%#10.8x\n",
475		reg_read(vcap, DCMIPP_P0DCCNTR));
476	dev_dbg(dev, "[DCMIPP_CMSR1] =%#10.8x\n", reg_read(vcap, DCMIPP_CMSR1));
477	dev_dbg(dev, "[DCMIPP_CMSR2] =%#10.8x\n", reg_read(vcap, DCMIPP_CMSR2));
478}
479
480/*
481 * Stop the stream engine. Any remaining buffers in the stream queue are
482 * dequeued and passed on to the vb2 framework marked as STATE_ERROR.
483 */
484static void dcmipp_bytecap_stop_streaming(struct vb2_queue *vq)
485{
486	struct dcmipp_bytecap_device *vcap = vb2_get_drv_priv(vq);
487	int ret;
488	u32 status;
489
490	ret = v4l2_subdev_disable_streams(vcap->s_subdev,
491					  vcap->s_subdev_pad_nb, BIT_ULL(0));
492	if (ret)
493		dev_warn(vcap->dev, "Failed to disable stream\n");
494
495	/* Stop the media pipeline */
496	media_pipeline_stop(vcap->vdev.entity.pads);
497
498	/* Disable interruptions */
499	reg_clear(vcap, DCMIPP_CMIER, vcap->cmier);
500
501	/* Stop capture */
502	reg_clear(vcap, DCMIPP_P0FCTCR, DCMIPP_P0FCTCR_CPTREQ);
503
504	/* Wait until CPTACT become 0 */
505	ret = readl_relaxed_poll_timeout(vcap->regs + DCMIPP_P0SR, status,
506					 !(status & DCMIPP_P0SR_CPTACT),
507					 20 * USEC_PER_MSEC,
508					 1000 * USEC_PER_MSEC);
509	if (ret)
510		dev_warn(vcap->dev, "Timeout when stopping\n");
511
512	/* Disable pipe */
513	reg_clear(vcap, DCMIPP_P0FSCR, DCMIPP_P0FSCR_PIPEN);
514
515	/* Clear any pending interrupts */
516	reg_write(vcap, DCMIPP_CMFCR, DCMIPP_CMIER_P0ALL);
517
518	spin_lock_irq(&vcap->irqlock);
519
520	/* Return all queued buffers to vb2 in ERROR state */
521	dcmipp_bytecap_all_buffers_done(vcap, VB2_BUF_STATE_ERROR);
522	INIT_LIST_HEAD(&vcap->buffers);
523
524	vcap->active = NULL;
525	vcap->state = DCMIPP_STOPPED;
526
527	spin_unlock_irq(&vcap->irqlock);
528
529	dcmipp_dump_status(vcap);
530
531	pm_runtime_put(vcap->dev);
532
533	if (vcap->count.errors)
534		dev_warn(vcap->dev, "Some errors found while streaming: errors=%d (overrun=%d, limit=%d, nactive=%d), underrun=%d, buffers=%d\n",
535			 vcap->count.errors, vcap->count.overrun,
536			 vcap->count.limit, vcap->count.nactive,
537			 vcap->count.underrun, vcap->count.buffers);
538}
539
540static int dcmipp_bytecap_buf_prepare(struct vb2_buffer *vb)
541{
542	struct dcmipp_bytecap_device *vcap =  vb2_get_drv_priv(vb->vb2_queue);
543	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
544	struct dcmipp_buf *buf = container_of(vbuf, struct dcmipp_buf, vb);
545	unsigned long size;
546
547	size = vcap->format.sizeimage;
548
549	if (vb2_plane_size(vb, 0) < size) {
550		dev_err(vcap->dev, "%s data will not fit into plane (%lu < %lu)\n",
551			__func__, vb2_plane_size(vb, 0), size);
552		return -EINVAL;
553	}
554
555	vb2_set_plane_payload(vb, 0, size);
556
557	if (!buf->prepared) {
558		/* Get memory addresses */
559		buf->addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
560		buf->size = vb2_plane_size(&buf->vb.vb2_buf, 0);
561		buf->prepared = true;
562
563		vb2_set_plane_payload(&buf->vb.vb2_buf, 0, buf->size);
564
565		dev_dbg(vcap->dev, "Setup [%d] phy=%pad size=%zu\n",
566			vb->index, &buf->addr, buf->size);
567	}
568
569	return 0;
570}
571
572static void dcmipp_bytecap_buf_queue(struct vb2_buffer *vb2_buf)
573{
574	struct dcmipp_bytecap_device *vcap =
575		vb2_get_drv_priv(vb2_buf->vb2_queue);
576	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb2_buf);
577	struct dcmipp_buf *buf = container_of(vbuf, struct dcmipp_buf, vb);
578
579	dev_dbg(vcap->dev, "Queue [%d] %p phy=%pad\n", buf->vb.vb2_buf.index,
580		buf, &buf->addr);
581
582	spin_lock_irq(&vcap->irqlock);
583	list_add_tail(&buf->list, &vcap->buffers);
584
585	if (vcap->state == DCMIPP_WAIT_FOR_BUFFER) {
586		vcap->next = buf;
587		dev_dbg(vcap->dev, "Restart with next [%d] %p phy=%pad\n",
588			buf->vb.vb2_buf.index, buf, &buf->addr);
589
590		dcmipp_start_capture(vcap, buf);
591
592		vcap->state = DCMIPP_RUNNING;
593	}
594
595	spin_unlock_irq(&vcap->irqlock);
596}
597
598static int dcmipp_bytecap_queue_setup(struct vb2_queue *vq,
599				      unsigned int *nbuffers,
600				      unsigned int *nplanes,
601				      unsigned int sizes[],
602				      struct device *alloc_devs[])
603{
604	struct dcmipp_bytecap_device *vcap = vb2_get_drv_priv(vq);
605	unsigned int size;
606
607	size = vcap->format.sizeimage;
608
609	/* Make sure the image size is large enough */
610	if (*nplanes)
611		return sizes[0] < vcap->format.sizeimage ? -EINVAL : 0;
612
613	*nplanes = 1;
614	sizes[0] = vcap->format.sizeimage;
615
616	dev_dbg(vcap->dev, "Setup queue, count=%d, size=%d\n",
617		*nbuffers, size);
618
619	return 0;
620}
621
622static int dcmipp_bytecap_buf_init(struct vb2_buffer *vb)
623{
624	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
625	struct dcmipp_buf *buf = container_of(vbuf, struct dcmipp_buf, vb);
626
627	INIT_LIST_HEAD(&buf->list);
628
629	return 0;
630}
631
632static const struct vb2_ops dcmipp_bytecap_qops = {
633	.start_streaming	= dcmipp_bytecap_start_streaming,
634	.stop_streaming		= dcmipp_bytecap_stop_streaming,
635	.buf_init		= dcmipp_bytecap_buf_init,
636	.buf_prepare		= dcmipp_bytecap_buf_prepare,
637	.buf_queue		= dcmipp_bytecap_buf_queue,
638	.queue_setup		= dcmipp_bytecap_queue_setup,
639};
640
641static void dcmipp_bytecap_release(struct video_device *vdev)
642{
643	struct dcmipp_bytecap_device *vcap =
644		container_of(vdev, struct dcmipp_bytecap_device, vdev);
645
646	dcmipp_pads_cleanup(vcap->ved.pads);
647	mutex_destroy(&vcap->lock);
648
649	kfree(vcap);
650}
651
652void dcmipp_bytecap_ent_release(struct dcmipp_ent_device *ved)
653{
654	struct dcmipp_bytecap_device *vcap =
655		container_of(ved, struct dcmipp_bytecap_device, ved);
656
657	media_entity_cleanup(ved->ent);
658	vb2_video_unregister_device(&vcap->vdev);
659}
660
661static void dcmipp_buffer_done(struct dcmipp_bytecap_device *vcap,
662			       struct dcmipp_buf *buf,
663			       size_t bytesused,
664			       int err)
665{
666	struct vb2_v4l2_buffer *vbuf;
667
668	list_del_init(&buf->list);
669
670	vbuf = &buf->vb;
671
672	vbuf->sequence = vcap->sequence++;
673	vbuf->field = V4L2_FIELD_NONE;
674	vbuf->vb2_buf.timestamp = ktime_get_ns();
675	vb2_set_plane_payload(&vbuf->vb2_buf, 0, bytesused);
676	vb2_buffer_done(&vbuf->vb2_buf,
677			err ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
678	dev_dbg(vcap->dev, "Done  [%d] %p phy=%pad\n", buf->vb.vb2_buf.index,
679		buf, &buf->addr);
680	vcap->count.buffers++;
681}
682
683/* irqlock must be held */
684static void
685dcmipp_bytecap_set_next_frame_or_stop(struct dcmipp_bytecap_device *vcap)
686{
687	if (!vcap->next && list_is_singular(&vcap->buffers)) {
688		/*
689		 * If there is no available buffer (none or a single one in the
690		 * list while two are expected), stop the capture (effective
691		 * for next frame). On-going frame capture will continue until
692		 * FRAME END but no further capture will be done.
693		 */
694		reg_clear(vcap, DCMIPP_P0FCTCR, DCMIPP_P0FCTCR_CPTREQ);
695
696		dev_dbg(vcap->dev, "Capture restart is deferred to next buffer queueing\n");
697		vcap->next = NULL;
698		vcap->state = DCMIPP_WAIT_FOR_BUFFER;
699		return;
700	}
701
702	/* If we don't have buffer yet, pick the one after active */
703	if (!vcap->next)
704		vcap->next = list_next_entry(vcap->active, list);
705
706	/*
707	 * Set buffer address
708	 * This register is shadowed and will be taken into
709	 * account on next VSYNC (start of next frame)
710	 */
711	reg_write(vcap, DCMIPP_P0PPM0AR1, vcap->next->addr);
712	dev_dbg(vcap->dev, "Write [%d] %p phy=%pad\n",
713		vcap->next->vb.vb2_buf.index, vcap->next, &vcap->next->addr);
714}
715
716/* irqlock must be held */
717static void dcmipp_bytecap_process_frame(struct dcmipp_bytecap_device *vcap,
718					 size_t bytesused)
719{
720	int err = 0;
721	struct dcmipp_buf *buf = vcap->active;
722
723	if (!buf) {
724		vcap->count.nactive++;
725		vcap->count.errors++;
726		return;
727	}
728
729	if (bytesused > buf->size) {
730		dev_dbg(vcap->dev, "frame larger than expected (%zu > %zu)\n",
731			bytesused, buf->size);
732		/* Clip to buffer size and return buffer to V4L2 in error */
733		bytesused = buf->size;
734		vcap->count.limit++;
735		vcap->count.errors++;
736		err = -EOVERFLOW;
737	}
738
739	dcmipp_buffer_done(vcap, buf, bytesused, err);
740	vcap->active = NULL;
741}
742
743static irqreturn_t dcmipp_bytecap_irq_thread(int irq, void *arg)
744{
745	struct dcmipp_bytecap_device *vcap =
746			container_of(arg, struct dcmipp_bytecap_device, ved);
747	size_t bytesused = 0;
748	u32 cmsr2;
749
750	spin_lock_irq(&vcap->irqlock);
751
752	cmsr2 = vcap->cmsr2 & vcap->cmier;
753
754	/*
755	 * If we have an overrun, a frame-end will probably not be generated,
756	 * in that case the active buffer will be recycled as next buffer by
757	 * the VSYNC handler
758	 */
759	if (cmsr2 & DCMIPP_CMSR2_P0OVRF) {
760		vcap->count.errors++;
761		vcap->count.overrun++;
762	}
763
764	if (cmsr2 & DCMIPP_CMSR2_P0FRAMEF) {
765		vcap->count.frame++;
766
767		/* Read captured buffer size */
768		bytesused = reg_read(vcap, DCMIPP_P0DCCNTR);
769		dcmipp_bytecap_process_frame(vcap, bytesused);
770	}
771
772	if (cmsr2 & DCMIPP_CMSR2_P0VSYNCF) {
773		vcap->count.vsync++;
774		if (vcap->state == DCMIPP_WAIT_FOR_BUFFER) {
775			vcap->count.underrun++;
776			goto out;
777		}
778
779		/*
780		 * On VSYNC, the previously set next buffer is going to become
781		 * active thanks to the shadowing mechanism of the DCMIPP. In
782		 * most of the cases, since a FRAMEEND has already come,
783		 * pointer next is NULL since active is reset during the
784		 * FRAMEEND handling. However, in case of framerate adjustment,
785		 * there are more VSYNC than FRAMEEND. Thus we recycle the
786		 * active (but not used) buffer and put it back into next.
787		 */
788		swap(vcap->active, vcap->next);
789		dcmipp_bytecap_set_next_frame_or_stop(vcap);
790	}
791
792out:
793	spin_unlock_irq(&vcap->irqlock);
794	return IRQ_HANDLED;
795}
796
797static irqreturn_t dcmipp_bytecap_irq_callback(int irq, void *arg)
798{
799	struct dcmipp_bytecap_device *vcap =
800			container_of(arg, struct dcmipp_bytecap_device, ved);
801
802	/* Store interrupt status register */
803	vcap->cmsr2 = reg_read(vcap, DCMIPP_CMSR2) & vcap->cmier;
804	vcap->count.it++;
805
806	/* Clear interrupt */
807	reg_write(vcap, DCMIPP_CMFCR, vcap->cmsr2);
808
809	return IRQ_WAKE_THREAD;
810}
811
812static int dcmipp_bytecap_link_validate(struct media_link *link)
813{
814	struct media_entity *entity = link->sink->entity;
815	struct video_device *vd = media_entity_to_video_device(entity);
816	struct dcmipp_bytecap_device *vcap = container_of(vd,
817					struct dcmipp_bytecap_device, vdev);
818	struct v4l2_subdev *source_sd =
819		media_entity_to_v4l2_subdev(link->source->entity);
820	struct v4l2_subdev_format source_fmt = {
821		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
822		.pad = link->source->index,
823	};
824	int ret, i;
825
826	ret = v4l2_subdev_call(source_sd, pad, get_fmt, NULL, &source_fmt);
827	if (ret < 0)
828		return 0;
829
830	if (source_fmt.format.width != vcap->format.width ||
831	    source_fmt.format.height != vcap->format.height) {
832		dev_err(vcap->dev, "Wrong width or height %ux%u (%ux%u expected)\n",
833			vcap->format.width, vcap->format.height,
834			source_fmt.format.width, source_fmt.format.height);
835		return -EINVAL;
836	}
837
838	for (i = 0; i < ARRAY_SIZE(dcmipp_bytecap_pix_map_list); i++) {
839		if (dcmipp_bytecap_pix_map_list[i].pixelformat ==
840			vcap->format.pixelformat &&
841		    dcmipp_bytecap_pix_map_list[i].code ==
842			source_fmt.format.code)
843			break;
844	}
845
846	if (i == ARRAY_SIZE(dcmipp_bytecap_pix_map_list)) {
847		dev_err(vcap->dev, "mbus code 0x%x do not match capture device format (0x%x)\n",
848			vcap->format.pixelformat, source_fmt.format.code);
849		return -EINVAL;
850	}
851
852	return 0;
853}
854
855static const struct media_entity_operations dcmipp_bytecap_entity_ops = {
856	.link_validate = dcmipp_bytecap_link_validate,
857};
858
859struct dcmipp_ent_device *dcmipp_bytecap_ent_init(struct device *dev,
860						  const char *entity_name,
861						  struct v4l2_device *v4l2_dev,
862						  void __iomem *regs)
863{
864	struct dcmipp_bytecap_device *vcap;
865	struct video_device *vdev;
866	struct vb2_queue *q;
867	const unsigned long pad_flag = MEDIA_PAD_FL_SINK;
868	int ret = 0;
869
870	/* Allocate the dcmipp_bytecap_device struct */
871	vcap = kzalloc(sizeof(*vcap), GFP_KERNEL);
872	if (!vcap)
873		return ERR_PTR(-ENOMEM);
874
875	/* Allocate the pads */
876	vcap->ved.pads = dcmipp_pads_init(1, &pad_flag);
877	if (IS_ERR(vcap->ved.pads)) {
878		ret = PTR_ERR(vcap->ved.pads);
879		goto err_free_vcap;
880	}
881
882	/* Initialize the media entity */
883	vcap->vdev.entity.name = entity_name;
884	vcap->vdev.entity.function = MEDIA_ENT_F_IO_V4L;
885	vcap->vdev.entity.ops = &dcmipp_bytecap_entity_ops;
886	ret = media_entity_pads_init(&vcap->vdev.entity, 1, vcap->ved.pads);
887	if (ret)
888		goto err_clean_pads;
889
890	/* Initialize the lock */
891	mutex_init(&vcap->lock);
892
893	/* Initialize the vb2 queue */
894	q = &vcap->queue;
895	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
896	q->io_modes = VB2_MMAP | VB2_DMABUF;
897	q->lock = &vcap->lock;
898	q->drv_priv = vcap;
899	q->buf_struct_size = sizeof(struct dcmipp_buf);
900	q->ops = &dcmipp_bytecap_qops;
901	q->mem_ops = &vb2_dma_contig_memops;
902	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
903	q->min_queued_buffers = 1;
904	q->dev = dev;
905
906	/* DCMIPP requires 16 bytes aligned buffers */
907	ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(32));
908	if (ret) {
909		dev_err(dev, "Failed to set DMA mask\n");
910		goto err_mutex_destroy;
911	}
912
913	ret = vb2_queue_init(q);
914	if (ret) {
915		dev_err(dev, "%s: vb2 queue init failed (err=%d)\n",
916			entity_name, ret);
917		goto err_clean_m_ent;
918	}
919
920	/* Initialize buffer list and its lock */
921	INIT_LIST_HEAD(&vcap->buffers);
922	spin_lock_init(&vcap->irqlock);
923
924	/* Set default frame format */
925	vcap->format = fmt_default;
926
927	/* Fill the dcmipp_ent_device struct */
928	vcap->ved.ent = &vcap->vdev.entity;
929	vcap->ved.handler = dcmipp_bytecap_irq_callback;
930	vcap->ved.thread_fn = dcmipp_bytecap_irq_thread;
931	vcap->dev = dev;
932	vcap->regs = regs;
933
934	/* Initialize the video_device struct */
935	vdev = &vcap->vdev;
936	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
937			    V4L2_CAP_IO_MC;
938	vdev->release = dcmipp_bytecap_release;
939	vdev->fops = &dcmipp_bytecap_fops;
940	vdev->ioctl_ops = &dcmipp_bytecap_ioctl_ops;
941	vdev->lock = &vcap->lock;
942	vdev->queue = q;
943	vdev->v4l2_dev = v4l2_dev;
944	strscpy(vdev->name, entity_name, sizeof(vdev->name));
945	video_set_drvdata(vdev, &vcap->ved);
946
947	/* Register the video_device with the v4l2 and the media framework */
948	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
949	if (ret) {
950		dev_err(dev, "%s: video register failed (err=%d)\n",
951			vcap->vdev.name, ret);
952		goto err_clean_m_ent;
953	}
954
955	return &vcap->ved;
956
957err_clean_m_ent:
958	media_entity_cleanup(&vcap->vdev.entity);
959err_mutex_destroy:
960	mutex_destroy(&vcap->lock);
961err_clean_pads:
962	dcmipp_pads_cleanup(vcap->ved.pads);
963err_free_vcap:
964	kfree(vcap);
965
966	return ERR_PTR(ret);
967}