1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Driver for STM32 Digital Camera Memory Interface
4  *
5  * Copyright (C) STMicroelectronics SA 2017
6  * Authors: Yannick Fertre <yannick.fertre@st.com>
7  *          Hugues Fruchet <hugues.fruchet@st.com>
8  *          for STMicroelectronics.
9  *
10  * This driver is based on atmel_isi.c
11  *
12  */
13 
14 #include <linux/clk.h>
15 #include <linux/completion.h>
16 #include <linux/delay.h>
17 #include <linux/dmaengine.h>
18 #include <linux/init.h>
19 #include <linux/interrupt.h>
20 #include <linux/kernel.h>
21 #include <linux/module.h>
22 #include <linux/of.h>
23 #include <linux/of_device.h>
24 #include <linux/of_graph.h>
25 #include <linux/pinctrl/consumer.h>
26 #include <linux/platform_device.h>
27 #include <linux/pm_runtime.h>
28 #include <linux/reset.h>
29 #include <linux/videodev2.h>
30 
31 #include <media/v4l2-ctrls.h>
32 #include <media/v4l2-dev.h>
33 #include <media/v4l2-device.h>
34 #include <media/v4l2-event.h>
35 #include <media/v4l2-fwnode.h>
36 #include <media/v4l2-image-sizes.h>
37 #include <media/v4l2-ioctl.h>
38 #include <media/v4l2-rect.h>
39 #include <media/videobuf2-dma-contig.h>
40 
41 #define DRV_NAME "stm32-dcmi"
42 
43 /* Registers offset for DCMI */
44 #define DCMI_CR		0x00 /* Control Register */
45 #define DCMI_SR		0x04 /* Status Register */
46 #define DCMI_RIS	0x08 /* Raw Interrupt Status register */
47 #define DCMI_IER	0x0C /* Interrupt Enable Register */
48 #define DCMI_MIS	0x10 /* Masked Interrupt Status register */
49 #define DCMI_ICR	0x14 /* Interrupt Clear Register */
50 #define DCMI_ESCR	0x18 /* Embedded Synchronization Code Register */
51 #define DCMI_ESUR	0x1C /* Embedded Synchronization Unmask Register */
52 #define DCMI_CWSTRT	0x20 /* Crop Window STaRT */
53 #define DCMI_CWSIZE	0x24 /* Crop Window SIZE */
54 #define DCMI_DR		0x28 /* Data Register */
55 #define DCMI_IDR	0x2C /* IDentifier Register */
56 
57 /* Bits definition for control register (DCMI_CR) */
58 #define CR_CAPTURE	BIT(0)
59 #define CR_CM		BIT(1)
60 #define CR_CROP		BIT(2)
61 #define CR_JPEG		BIT(3)
62 #define CR_ESS		BIT(4)
63 #define CR_PCKPOL	BIT(5)
64 #define CR_HSPOL	BIT(6)
65 #define CR_VSPOL	BIT(7)
66 #define CR_FCRC_0	BIT(8)
67 #define CR_FCRC_1	BIT(9)
68 #define CR_EDM_0	BIT(10)
69 #define CR_EDM_1	BIT(11)
70 #define CR_ENABLE	BIT(14)
71 
72 /* Bits definition for status register (DCMI_SR) */
73 #define SR_HSYNC	BIT(0)
74 #define SR_VSYNC	BIT(1)
75 #define SR_FNE		BIT(2)
76 
77 /*
78  * Bits definition for interrupt registers
79  * (DCMI_RIS, DCMI_IER, DCMI_MIS, DCMI_ICR)
80  */
81 #define IT_FRAME	BIT(0)
82 #define IT_OVR		BIT(1)
83 #define IT_ERR		BIT(2)
84 #define IT_VSYNC	BIT(3)
85 #define IT_LINE		BIT(4)
86 
87 enum state {
88 	STOPPED = 0,
89 	WAIT_FOR_BUFFER,
90 	RUNNING,
91 };
92 
93 #define MIN_WIDTH	16U
94 #define MAX_WIDTH	2592U
95 #define MIN_HEIGHT	16U
96 #define MAX_HEIGHT	2592U
97 
98 #define TIMEOUT_MS	1000
99 
100 #define OVERRUN_ERROR_THRESHOLD	3
101 
102 struct dcmi_format {
103 	u32	fourcc;
104 	u32	mbus_code;
105 	u8	bpp;
106 };
107 
108 struct dcmi_framesize {
109 	u32	width;
110 	u32	height;
111 };
112 
113 struct dcmi_buf {
114 	struct vb2_v4l2_buffer	vb;
115 	bool			prepared;
116 	struct sg_table		sgt;
117 	size_t			size;
118 	struct list_head	list;
119 };
120 
121 struct stm32_dcmi {
122 	/* Protects the access of variables shared within the interrupt */
123 	spinlock_t			irqlock;
124 	struct device			*dev;
125 	void __iomem			*regs;
126 	struct resource			*res;
127 	struct reset_control		*rstc;
128 	int				sequence;
129 	struct list_head		buffers;
130 	struct dcmi_buf			*active;
131 	int			irq;
132 
133 	struct v4l2_device		v4l2_dev;
134 	struct video_device		*vdev;
135 	struct v4l2_async_notifier	notifier;
136 	struct v4l2_subdev		*source;
137 	struct v4l2_subdev		*s_subdev;
138 	struct v4l2_format		fmt;
139 	struct v4l2_rect		crop;
140 	bool				do_crop;
141 
142 	const struct dcmi_format	**sd_formats;
143 	unsigned int			num_of_sd_formats;
144 	const struct dcmi_format	*sd_format;
145 	struct dcmi_framesize		*sd_framesizes;
146 	unsigned int			num_of_sd_framesizes;
147 	struct dcmi_framesize		sd_framesize;
148 	struct v4l2_rect		sd_bounds;
149 
150 	/* Protect this data structure */
151 	struct mutex			lock;
152 	struct vb2_queue		queue;
153 
154 	struct v4l2_mbus_config_parallel	bus;
155 	enum v4l2_mbus_type		bus_type;
156 	struct completion		complete;
157 	struct clk			*mclk;
158 	enum state			state;
159 	struct dma_chan			*dma_chan;
160 	dma_cookie_t			dma_cookie;
161 	u32				dma_max_burst;
162 	u32				misr;
163 	int				errors_count;
164 	int				overrun_count;
165 	int				buffers_count;
166 
167 	/* Ensure DMA operations atomicity */
168 	struct mutex			dma_lock;
169 
170 	struct media_device		mdev;
171 	struct media_pad		vid_cap_pad;
172 	struct media_pipeline		pipeline;
173 };
174 
175 static inline struct stm32_dcmi *notifier_to_dcmi(struct v4l2_async_notifier *n)
176 {
177 	return container_of(n, struct stm32_dcmi, notifier);
178 }
179 
180 static inline u32 reg_read(void __iomem *base, u32 reg)
181 {
182 	return readl_relaxed(base + reg);
183 }
184 
185 static inline void reg_write(void __iomem *base, u32 reg, u32 val)
186 {
187 	writel_relaxed(val, base + reg);
188 }
189 
190 static inline void reg_set(void __iomem *base, u32 reg, u32 mask)
191 {
192 	reg_write(base, reg, reg_read(base, reg) | mask);
193 }
194 
195 static inline void reg_clear(void __iomem *base, u32 reg, u32 mask)
196 {
197 	reg_write(base, reg, reg_read(base, reg) & ~mask);
198 }
199 
200 static int dcmi_start_capture(struct stm32_dcmi *dcmi, struct dcmi_buf *buf);
201 
202 static void dcmi_buffer_done(struct stm32_dcmi *dcmi,
203 			     struct dcmi_buf *buf,
204 			     size_t bytesused,
205 			     int err)
206 {
207 	struct vb2_v4l2_buffer *vbuf;
208 
209 	if (!buf)
210 		return;
211 
212 	list_del_init(&buf->list);
213 
214 	vbuf = &buf->vb;
215 
216 	vbuf->sequence = dcmi->sequence++;
217 	vbuf->field = V4L2_FIELD_NONE;
218 	vbuf->vb2_buf.timestamp = ktime_get_ns();
219 	vb2_set_plane_payload(&vbuf->vb2_buf, 0, bytesused);
220 	vb2_buffer_done(&vbuf->vb2_buf,
221 			err ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
222 	dev_dbg(dcmi->dev, "buffer[%d] done seq=%d, bytesused=%zu\n",
223 		vbuf->vb2_buf.index, vbuf->sequence, bytesused);
224 
225 	dcmi->buffers_count++;
226 	dcmi->active = NULL;
227 }
228 
229 static int dcmi_restart_capture(struct stm32_dcmi *dcmi)
230 {
231 	struct dcmi_buf *buf;
232 
233 	spin_lock_irq(&dcmi->irqlock);
234 
235 	if (dcmi->state != RUNNING) {
236 		spin_unlock_irq(&dcmi->irqlock);
237 		return -EINVAL;
238 	}
239 
240 	/* Restart a new DMA transfer with next buffer */
241 	if (list_empty(&dcmi->buffers)) {
242 		dev_dbg(dcmi->dev, "Capture restart is deferred to next buffer queueing\n");
243 		dcmi->state = WAIT_FOR_BUFFER;
244 		spin_unlock_irq(&dcmi->irqlock);
245 		return 0;
246 	}
247 	buf = list_entry(dcmi->buffers.next, struct dcmi_buf, list);
248 	dcmi->active = buf;
249 
250 	spin_unlock_irq(&dcmi->irqlock);
251 
252 	return dcmi_start_capture(dcmi, buf);
253 }
254 
255 static void dcmi_dma_callback(void *param)
256 {
257 	struct stm32_dcmi *dcmi = (struct stm32_dcmi *)param;
258 	struct dma_tx_state state;
259 	enum dma_status status;
260 	struct dcmi_buf *buf = dcmi->active;
261 
262 	spin_lock_irq(&dcmi->irqlock);
263 
264 	/* Check DMA status */
265 	status = dmaengine_tx_status(dcmi->dma_chan, dcmi->dma_cookie, &state);
266 
267 	switch (status) {
268 	case DMA_IN_PROGRESS:
269 		dev_dbg(dcmi->dev, "%s: Received DMA_IN_PROGRESS\n", __func__);
270 		break;
271 	case DMA_PAUSED:
272 		dev_err(dcmi->dev, "%s: Received DMA_PAUSED\n", __func__);
273 		break;
274 	case DMA_ERROR:
275 		dev_err(dcmi->dev, "%s: Received DMA_ERROR\n", __func__);
276 
277 		/* Return buffer to V4L2 in error state */
278 		dcmi_buffer_done(dcmi, buf, 0, -EIO);
279 		break;
280 	case DMA_COMPLETE:
281 		dev_dbg(dcmi->dev, "%s: Received DMA_COMPLETE\n", __func__);
282 
283 		/* Return buffer to V4L2 */
284 		dcmi_buffer_done(dcmi, buf, buf->size, 0);
285 
286 		spin_unlock_irq(&dcmi->irqlock);
287 
288 		/* Restart capture */
289 		if (dcmi_restart_capture(dcmi))
290 			dev_err(dcmi->dev, "%s: Cannot restart capture on DMA complete\n",
291 				__func__);
292 		return;
293 	default:
294 		dev_err(dcmi->dev, "%s: Received unknown status\n", __func__);
295 		break;
296 	}
297 
298 	spin_unlock_irq(&dcmi->irqlock);
299 }
300 
301 static int dcmi_start_dma(struct stm32_dcmi *dcmi,
302 			  struct dcmi_buf *buf)
303 {
304 	struct dma_async_tx_descriptor *desc = NULL;
305 	struct dma_slave_config config;
306 	int ret;
307 
308 	memset(&config, 0, sizeof(config));
309 
310 	config.src_addr = (dma_addr_t)dcmi->res->start + DCMI_DR;
311 	config.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
312 	config.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
313 	config.dst_maxburst = 4;
314 
315 	/* Configure DMA channel */
316 	ret = dmaengine_slave_config(dcmi->dma_chan, &config);
317 	if (ret < 0) {
318 		dev_err(dcmi->dev, "%s: DMA channel config failed (%d)\n",
319 			__func__, ret);
320 		return ret;
321 	}
322 
323 	/*
324 	 * Avoid call of dmaengine_terminate_sync() between
325 	 * dmaengine_prep_slave_single() and dmaengine_submit()
326 	 * by locking the whole DMA submission sequence
327 	 */
328 	mutex_lock(&dcmi->dma_lock);
329 
330 	/* Prepare a DMA transaction */
331 	desc = dmaengine_prep_slave_sg(dcmi->dma_chan, buf->sgt.sgl, buf->sgt.nents,
332 				       DMA_DEV_TO_MEM,
333 				       DMA_PREP_INTERRUPT);
334 	if (!desc) {
335 		dev_err(dcmi->dev, "%s: DMA dmaengine_prep_slave_sg failed\n", __func__);
336 		mutex_unlock(&dcmi->dma_lock);
337 		return -EINVAL;
338 	}
339 
340 	/* Set completion callback routine for notification */
341 	desc->callback = dcmi_dma_callback;
342 	desc->callback_param = dcmi;
343 
344 	/* Push current DMA transaction in the pending queue */
345 	dcmi->dma_cookie = dmaengine_submit(desc);
346 	if (dma_submit_error(dcmi->dma_cookie)) {
347 		dev_err(dcmi->dev, "%s: DMA submission failed\n", __func__);
348 		mutex_unlock(&dcmi->dma_lock);
349 		return -ENXIO;
350 	}
351 
352 	mutex_unlock(&dcmi->dma_lock);
353 
354 	dma_async_issue_pending(dcmi->dma_chan);
355 
356 	return 0;
357 }
358 
359 static int dcmi_start_capture(struct stm32_dcmi *dcmi, struct dcmi_buf *buf)
360 {
361 	int ret;
362 
363 	if (!buf)
364 		return -EINVAL;
365 
366 	ret = dcmi_start_dma(dcmi, buf);
367 	if (ret) {
368 		dcmi->errors_count++;
369 		return ret;
370 	}
371 
372 	/* Enable capture */
373 	reg_set(dcmi->regs, DCMI_CR, CR_CAPTURE);
374 
375 	return 0;
376 }
377 
378 static void dcmi_set_crop(struct stm32_dcmi *dcmi)
379 {
380 	u32 size, start;
381 
382 	/* Crop resolution */
383 	size = ((dcmi->crop.height - 1) << 16) |
384 		((dcmi->crop.width << 1) - 1);
385 	reg_write(dcmi->regs, DCMI_CWSIZE, size);
386 
387 	/* Crop start point */
388 	start = ((dcmi->crop.top) << 16) |
389 		 ((dcmi->crop.left << 1));
390 	reg_write(dcmi->regs, DCMI_CWSTRT, start);
391 
392 	dev_dbg(dcmi->dev, "Cropping to %ux%u@%u:%u\n",
393 		dcmi->crop.width, dcmi->crop.height,
394 		dcmi->crop.left, dcmi->crop.top);
395 
396 	/* Enable crop */
397 	reg_set(dcmi->regs, DCMI_CR, CR_CROP);
398 }
399 
400 static void dcmi_process_jpeg(struct stm32_dcmi *dcmi)
401 {
402 	struct dma_tx_state state;
403 	enum dma_status status;
404 	struct dcmi_buf *buf = dcmi->active;
405 
406 	if (!buf)
407 		return;
408 
409 	/*
410 	 * Because of variable JPEG buffer size sent by sensor,
411 	 * DMA transfer never completes due to transfer size never reached.
412 	 * In order to ensure that all the JPEG data are transferred
413 	 * in active buffer memory, DMA is drained.
414 	 * Then DMA tx status gives the amount of data transferred
415 	 * to memory, which is then returned to V4L2 through the active
416 	 * buffer payload.
417 	 */
418 
419 	/* Drain DMA */
420 	dmaengine_synchronize(dcmi->dma_chan);
421 
422 	/* Get DMA residue to get JPEG size */
423 	status = dmaengine_tx_status(dcmi->dma_chan, dcmi->dma_cookie, &state);
424 	if (status != DMA_ERROR && state.residue < buf->size) {
425 		/* Return JPEG buffer to V4L2 with received JPEG buffer size */
426 		dcmi_buffer_done(dcmi, buf, buf->size - state.residue, 0);
427 	} else {
428 		dcmi->errors_count++;
429 		dev_err(dcmi->dev, "%s: Cannot get JPEG size from DMA\n",
430 			__func__);
431 		/* Return JPEG buffer to V4L2 in ERROR state */
432 		dcmi_buffer_done(dcmi, buf, 0, -EIO);
433 	}
434 
435 	/* Abort DMA operation */
436 	dmaengine_terminate_sync(dcmi->dma_chan);
437 
438 	/* Restart capture */
439 	if (dcmi_restart_capture(dcmi))
440 		dev_err(dcmi->dev, "%s: Cannot restart capture on JPEG received\n",
441 			__func__);
442 }
443 
444 static irqreturn_t dcmi_irq_thread(int irq, void *arg)
445 {
446 	struct stm32_dcmi *dcmi = arg;
447 
448 	spin_lock_irq(&dcmi->irqlock);
449 
450 	if (dcmi->misr & IT_OVR) {
451 		dcmi->overrun_count++;
452 		if (dcmi->overrun_count > OVERRUN_ERROR_THRESHOLD)
453 			dcmi->errors_count++;
454 	}
455 	if (dcmi->misr & IT_ERR)
456 		dcmi->errors_count++;
457 
458 	if (dcmi->sd_format->fourcc == V4L2_PIX_FMT_JPEG &&
459 	    dcmi->misr & IT_FRAME) {
460 		/* JPEG received */
461 		spin_unlock_irq(&dcmi->irqlock);
462 		dcmi_process_jpeg(dcmi);
463 		return IRQ_HANDLED;
464 	}
465 
466 	spin_unlock_irq(&dcmi->irqlock);
467 	return IRQ_HANDLED;
468 }
469 
470 static irqreturn_t dcmi_irq_callback(int irq, void *arg)
471 {
472 	struct stm32_dcmi *dcmi = arg;
473 	unsigned long flags;
474 
475 	spin_lock_irqsave(&dcmi->irqlock, flags);
476 
477 	dcmi->misr = reg_read(dcmi->regs, DCMI_MIS);
478 
479 	/* Clear interrupt */
480 	reg_set(dcmi->regs, DCMI_ICR, IT_FRAME | IT_OVR | IT_ERR);
481 
482 	spin_unlock_irqrestore(&dcmi->irqlock, flags);
483 
484 	return IRQ_WAKE_THREAD;
485 }
486 
487 static int dcmi_queue_setup(struct vb2_queue *vq,
488 			    unsigned int *nbuffers,
489 			    unsigned int *nplanes,
490 			    unsigned int sizes[],
491 			    struct device *alloc_devs[])
492 {
493 	struct stm32_dcmi *dcmi = vb2_get_drv_priv(vq);
494 	unsigned int size;
495 
496 	size = dcmi->fmt.fmt.pix.sizeimage;
497 
498 	/* Make sure the image size is large enough */
499 	if (*nplanes)
500 		return sizes[0] < size ? -EINVAL : 0;
501 
502 	*nplanes = 1;
503 	sizes[0] = size;
504 
505 	dev_dbg(dcmi->dev, "Setup queue, count=%d, size=%d\n",
506 		*nbuffers, size);
507 
508 	return 0;
509 }
510 
511 static int dcmi_buf_init(struct vb2_buffer *vb)
512 {
513 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
514 	struct dcmi_buf *buf = container_of(vbuf, struct dcmi_buf, vb);
515 
516 	INIT_LIST_HEAD(&buf->list);
517 
518 	return 0;
519 }
520 
521 static int dcmi_buf_prepare(struct vb2_buffer *vb)
522 {
523 	struct stm32_dcmi *dcmi =  vb2_get_drv_priv(vb->vb2_queue);
524 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
525 	struct dcmi_buf *buf = container_of(vbuf, struct dcmi_buf, vb);
526 	unsigned long size;
527 	unsigned int num_sgs = 1;
528 	dma_addr_t dma_buf;
529 	struct scatterlist *sg;
530 	int i, ret;
531 
532 	size = dcmi->fmt.fmt.pix.sizeimage;
533 
534 	if (vb2_plane_size(vb, 0) < size) {
535 		dev_err(dcmi->dev, "%s data will not fit into plane (%lu < %lu)\n",
536 			__func__, vb2_plane_size(vb, 0), size);
537 		return -EINVAL;
538 	}
539 
540 	vb2_set_plane_payload(vb, 0, size);
541 
542 	if (!buf->prepared) {
543 		/* Get memory addresses */
544 		buf->size = vb2_plane_size(&buf->vb.vb2_buf, 0);
545 		if (buf->size > dcmi->dma_max_burst)
546 			num_sgs = DIV_ROUND_UP(buf->size, dcmi->dma_max_burst);
547 
548 		ret = sg_alloc_table(&buf->sgt, num_sgs, GFP_ATOMIC);
549 		if (ret) {
550 			dev_err(dcmi->dev, "sg table alloc failed\n");
551 			return ret;
552 		}
553 
554 		dma_buf = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
555 
556 		dev_dbg(dcmi->dev, "buffer[%d] phy=%pad size=%zu\n",
557 			vb->index, &dma_buf, buf->size);
558 
559 		for_each_sg(buf->sgt.sgl, sg, num_sgs, i) {
560 			size_t bytes = min_t(size_t, size, dcmi->dma_max_burst);
561 
562 			sg_dma_address(sg) = dma_buf;
563 			sg_dma_len(sg) = bytes;
564 			dma_buf += bytes;
565 			size -= bytes;
566 		}
567 
568 		buf->prepared = true;
569 
570 		vb2_set_plane_payload(&buf->vb.vb2_buf, 0, buf->size);
571 	}
572 
573 	return 0;
574 }
575 
576 static void dcmi_buf_queue(struct vb2_buffer *vb)
577 {
578 	struct stm32_dcmi *dcmi =  vb2_get_drv_priv(vb->vb2_queue);
579 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
580 	struct dcmi_buf *buf = container_of(vbuf, struct dcmi_buf, vb);
581 
582 	spin_lock_irq(&dcmi->irqlock);
583 
584 	/* Enqueue to video buffers list */
585 	list_add_tail(&buf->list, &dcmi->buffers);
586 
587 	if (dcmi->state == WAIT_FOR_BUFFER) {
588 		dcmi->state = RUNNING;
589 		dcmi->active = buf;
590 
591 		dev_dbg(dcmi->dev, "Starting capture on buffer[%d] queued\n",
592 			buf->vb.vb2_buf.index);
593 
594 		spin_unlock_irq(&dcmi->irqlock);
595 		if (dcmi_start_capture(dcmi, buf))
596 			dev_err(dcmi->dev, "%s: Cannot restart capture on overflow or error\n",
597 				__func__);
598 		return;
599 	}
600 
601 	spin_unlock_irq(&dcmi->irqlock);
602 }
603 
604 static struct media_entity *dcmi_find_source(struct stm32_dcmi *dcmi)
605 {
606 	struct media_entity *entity = &dcmi->vdev->entity;
607 	struct media_pad *pad;
608 
609 	/* Walk searching for entity having no sink */
610 	while (1) {
611 		pad = &entity->pads[0];
612 		if (!(pad->flags & MEDIA_PAD_FL_SINK))
613 			break;
614 
615 		pad = media_pad_remote_pad_first(pad);
616 		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
617 			break;
618 
619 		entity = pad->entity;
620 	}
621 
622 	return entity;
623 }
624 
625 static int dcmi_pipeline_s_fmt(struct stm32_dcmi *dcmi,
626 			       struct v4l2_subdev_format *format)
627 {
628 	struct media_entity *entity = &dcmi->source->entity;
629 	struct v4l2_subdev *subdev;
630 	struct media_pad *sink_pad = NULL;
631 	struct media_pad *src_pad = NULL;
632 	struct media_pad *pad = NULL;
633 	struct v4l2_subdev_format fmt = *format;
634 	bool found = false;
635 	int ret;
636 
637 	/*
638 	 * Starting from sensor subdevice, walk within
639 	 * pipeline and set format on each subdevice
640 	 */
641 	while (1) {
642 		unsigned int i;
643 
644 		/* Search if current entity has a source pad */
645 		for (i = 0; i < entity->num_pads; i++) {
646 			pad = &entity->pads[i];
647 			if (pad->flags & MEDIA_PAD_FL_SOURCE) {
648 				src_pad = pad;
649 				found = true;
650 				break;
651 			}
652 		}
653 		if (!found)
654 			break;
655 
656 		subdev = media_entity_to_v4l2_subdev(entity);
657 
658 		/* Propagate format on sink pad if any, otherwise source pad */
659 		if (sink_pad)
660 			pad = sink_pad;
661 
662 		dev_dbg(dcmi->dev, "\"%s\":%d pad format set to 0x%x %ux%u\n",
663 			subdev->name, pad->index, format->format.code,
664 			format->format.width, format->format.height);
665 
666 		fmt.pad = pad->index;
667 		ret = v4l2_subdev_call(subdev, pad, set_fmt, NULL, &fmt);
668 		if (ret < 0) {
669 			dev_err(dcmi->dev, "%s: Failed to set format 0x%x %ux%u on \"%s\":%d pad (%d)\n",
670 				__func__, format->format.code,
671 				format->format.width, format->format.height,
672 				subdev->name, pad->index, ret);
673 			return ret;
674 		}
675 
676 		if (fmt.format.code != format->format.code ||
677 		    fmt.format.width != format->format.width ||
678 		    fmt.format.height != format->format.height) {
679 			dev_dbg(dcmi->dev, "\"%s\":%d pad format has been changed to 0x%x %ux%u\n",
680 				subdev->name, pad->index, fmt.format.code,
681 				fmt.format.width, fmt.format.height);
682 		}
683 
684 		/* Walk to next entity */
685 		sink_pad = media_pad_remote_pad_first(src_pad);
686 		if (!sink_pad || !is_media_entity_v4l2_subdev(sink_pad->entity))
687 			break;
688 
689 		entity = sink_pad->entity;
690 	}
691 	*format = fmt;
692 
693 	return 0;
694 }
695 
696 static int dcmi_start_streaming(struct vb2_queue *vq, unsigned int count)
697 {
698 	struct stm32_dcmi *dcmi = vb2_get_drv_priv(vq);
699 	struct dcmi_buf *buf, *node;
700 	u32 val = 0;
701 	int ret;
702 
703 	ret = pm_runtime_resume_and_get(dcmi->dev);
704 	if (ret < 0) {
705 		dev_err(dcmi->dev, "%s: Failed to start streaming, cannot get sync (%d)\n",
706 			__func__, ret);
707 		goto err_unlocked;
708 	}
709 
710 	ret = video_device_pipeline_start(dcmi->vdev, &dcmi->pipeline);
711 	if (ret < 0) {
712 		dev_err(dcmi->dev, "%s: Failed to start streaming, media pipeline start error (%d)\n",
713 			__func__, ret);
714 		goto err_pm_put;
715 	}
716 
717 	ret = v4l2_subdev_call(dcmi->s_subdev, video, s_stream, 1);
718 	if (ret < 0) {
719 		dev_err(dcmi->dev, "%s: Failed to start source subdev, error (%d)\n",
720 			__func__, ret);
721 		goto err_media_pipeline_stop;
722 	}
723 
724 	spin_lock_irq(&dcmi->irqlock);
725 
726 	/* Set bus width */
727 	switch (dcmi->bus.bus_width) {
728 	case 14:
729 		val |= CR_EDM_0 | CR_EDM_1;
730 		break;
731 	case 12:
732 		val |= CR_EDM_1;
733 		break;
734 	case 10:
735 		val |= CR_EDM_0;
736 		break;
737 	default:
738 		/* Set bus width to 8 bits by default */
739 		break;
740 	}
741 
742 	/* Set vertical synchronization polarity */
743 	if (dcmi->bus.flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
744 		val |= CR_VSPOL;
745 
746 	/* Set horizontal synchronization polarity */
747 	if (dcmi->bus.flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
748 		val |= CR_HSPOL;
749 
750 	/* Set pixel clock polarity */
751 	if (dcmi->bus.flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
752 		val |= CR_PCKPOL;
753 
754 	/*
755 	 * BT656 embedded synchronisation bus mode.
756 	 *
757 	 * Default SAV/EAV mode is supported here with default codes
758 	 * SAV=0xff000080 & EAV=0xff00009d.
759 	 * With DCMI this means LSC=SAV=0x80 & LEC=EAV=0x9d.
760 	 */
761 	if (dcmi->bus_type == V4L2_MBUS_BT656) {
762 		val |= CR_ESS;
763 
764 		/* Unmask all codes */
765 		reg_write(dcmi->regs, DCMI_ESUR, 0xffffffff);/* FEC:LEC:LSC:FSC */
766 
767 		/* Trig on LSC=0x80 & LEC=0x9d codes, ignore FSC and FEC */
768 		reg_write(dcmi->regs, DCMI_ESCR, 0xff9d80ff);/* FEC:LEC:LSC:FSC */
769 	}
770 
771 	reg_write(dcmi->regs, DCMI_CR, val);
772 
773 	/* Set crop */
774 	if (dcmi->do_crop)
775 		dcmi_set_crop(dcmi);
776 
777 	/* Enable jpeg capture */
778 	if (dcmi->sd_format->fourcc == V4L2_PIX_FMT_JPEG)
779 		reg_set(dcmi->regs, DCMI_CR, CR_CM);/* Snapshot mode */
780 
781 	/* Enable dcmi */
782 	reg_set(dcmi->regs, DCMI_CR, CR_ENABLE);
783 
784 	dcmi->sequence = 0;
785 	dcmi->errors_count = 0;
786 	dcmi->overrun_count = 0;
787 	dcmi->buffers_count = 0;
788 
789 	/*
790 	 * Start transfer if at least one buffer has been queued,
791 	 * otherwise transfer is deferred at buffer queueing
792 	 */
793 	if (list_empty(&dcmi->buffers)) {
794 		dev_dbg(dcmi->dev, "Start streaming is deferred to next buffer queueing\n");
795 		dcmi->state = WAIT_FOR_BUFFER;
796 		spin_unlock_irq(&dcmi->irqlock);
797 		return 0;
798 	}
799 
800 	buf = list_entry(dcmi->buffers.next, struct dcmi_buf, list);
801 	dcmi->active = buf;
802 
803 	dcmi->state = RUNNING;
804 
805 	dev_dbg(dcmi->dev, "Start streaming, starting capture\n");
806 
807 	spin_unlock_irq(&dcmi->irqlock);
808 	ret = dcmi_start_capture(dcmi, buf);
809 	if (ret) {
810 		dev_err(dcmi->dev, "%s: Start streaming failed, cannot start capture\n",
811 			__func__);
812 		goto err_pipeline_stop;
813 	}
814 
815 	/* Enable interruptions */
816 	if (dcmi->sd_format->fourcc == V4L2_PIX_FMT_JPEG)
817 		reg_set(dcmi->regs, DCMI_IER, IT_FRAME | IT_OVR | IT_ERR);
818 	else
819 		reg_set(dcmi->regs, DCMI_IER, IT_OVR | IT_ERR);
820 
821 	return 0;
822 
823 err_pipeline_stop:
824 	v4l2_subdev_call(dcmi->s_subdev, video, s_stream, 0);
825 
826 err_media_pipeline_stop:
827 	video_device_pipeline_stop(dcmi->vdev);
828 
829 err_pm_put:
830 	pm_runtime_put(dcmi->dev);
831 err_unlocked:
832 	spin_lock_irq(&dcmi->irqlock);
833 	/*
834 	 * Return all buffers to vb2 in QUEUED state.
835 	 * This will give ownership back to userspace
836 	 */
837 	list_for_each_entry_safe(buf, node, &dcmi->buffers, list) {
838 		list_del_init(&buf->list);
839 		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
840 	}
841 	dcmi->active = NULL;
842 	spin_unlock_irq(&dcmi->irqlock);
843 
844 	return ret;
845 }
846 
847 static void dcmi_stop_streaming(struct vb2_queue *vq)
848 {
849 	struct stm32_dcmi *dcmi = vb2_get_drv_priv(vq);
850 	struct dcmi_buf *buf, *node;
851 	int ret;
852 
853 	ret = v4l2_subdev_call(dcmi->s_subdev, video, s_stream, 0);
854 	if (ret < 0)
855 		dev_err(dcmi->dev, "%s: Failed to stop source subdev, error (%d)\n",
856 			__func__, ret);
857 
858 	video_device_pipeline_stop(dcmi->vdev);
859 
860 	spin_lock_irq(&dcmi->irqlock);
861 
862 	/* Disable interruptions */
863 	reg_clear(dcmi->regs, DCMI_IER, IT_FRAME | IT_OVR | IT_ERR);
864 
865 	/* Disable DCMI */
866 	reg_clear(dcmi->regs, DCMI_CR, CR_ENABLE);
867 
868 	/* Return all queued buffers to vb2 in ERROR state */
869 	list_for_each_entry_safe(buf, node, &dcmi->buffers, list) {
870 		list_del_init(&buf->list);
871 		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
872 	}
873 
874 	dcmi->active = NULL;
875 	dcmi->state = STOPPED;
876 
877 	spin_unlock_irq(&dcmi->irqlock);
878 
879 	/* Stop all pending DMA operations */
880 	mutex_lock(&dcmi->dma_lock);
881 	dmaengine_terminate_sync(dcmi->dma_chan);
882 	mutex_unlock(&dcmi->dma_lock);
883 
884 	pm_runtime_put(dcmi->dev);
885 
886 	if (dcmi->errors_count)
887 		dev_warn(dcmi->dev, "Some errors found while streaming: errors=%d (overrun=%d), buffers=%d\n",
888 			 dcmi->errors_count, dcmi->overrun_count,
889 			 dcmi->buffers_count);
890 	dev_dbg(dcmi->dev, "Stop streaming, errors=%d (overrun=%d), buffers=%d\n",
891 		dcmi->errors_count, dcmi->overrun_count,
892 		dcmi->buffers_count);
893 }
894 
895 static const struct vb2_ops dcmi_video_qops = {
896 	.queue_setup		= dcmi_queue_setup,
897 	.buf_init		= dcmi_buf_init,
898 	.buf_prepare		= dcmi_buf_prepare,
899 	.buf_queue		= dcmi_buf_queue,
900 	.start_streaming	= dcmi_start_streaming,
901 	.stop_streaming		= dcmi_stop_streaming,
902 	.wait_prepare		= vb2_ops_wait_prepare,
903 	.wait_finish		= vb2_ops_wait_finish,
904 };
905 
906 static int dcmi_g_fmt_vid_cap(struct file *file, void *priv,
907 			      struct v4l2_format *fmt)
908 {
909 	struct stm32_dcmi *dcmi = video_drvdata(file);
910 
911 	*fmt = dcmi->fmt;
912 
913 	return 0;
914 }
915 
916 static const struct dcmi_format *find_format_by_fourcc(struct stm32_dcmi *dcmi,
917 						       unsigned int fourcc)
918 {
919 	unsigned int num_formats = dcmi->num_of_sd_formats;
920 	const struct dcmi_format *fmt;
921 	unsigned int i;
922 
923 	for (i = 0; i < num_formats; i++) {
924 		fmt = dcmi->sd_formats[i];
925 		if (fmt->fourcc == fourcc)
926 			return fmt;
927 	}
928 
929 	return NULL;
930 }
931 
932 static void __find_outer_frame_size(struct stm32_dcmi *dcmi,
933 				    struct v4l2_pix_format *pix,
934 				    struct dcmi_framesize *framesize)
935 {
936 	struct dcmi_framesize *match = NULL;
937 	unsigned int i;
938 	unsigned int min_err = UINT_MAX;
939 
940 	for (i = 0; i < dcmi->num_of_sd_framesizes; i++) {
941 		struct dcmi_framesize *fsize = &dcmi->sd_framesizes[i];
942 		int w_err = (fsize->width - pix->width);
943 		int h_err = (fsize->height - pix->height);
944 		int err = w_err + h_err;
945 
946 		if (w_err >= 0 && h_err >= 0 && err < min_err) {
947 			min_err = err;
948 			match = fsize;
949 		}
950 	}
951 	if (!match)
952 		match = &dcmi->sd_framesizes[0];
953 
954 	*framesize = *match;
955 }
956 
957 static int dcmi_try_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f,
958 			const struct dcmi_format **sd_format,
959 			struct dcmi_framesize *sd_framesize)
960 {
961 	const struct dcmi_format *sd_fmt;
962 	struct dcmi_framesize sd_fsize;
963 	struct v4l2_pix_format *pix = &f->fmt.pix;
964 	struct v4l2_subdev_format format = {
965 		.which = V4L2_SUBDEV_FORMAT_TRY,
966 	};
967 	bool do_crop;
968 	int ret;
969 
970 	sd_fmt = find_format_by_fourcc(dcmi, pix->pixelformat);
971 	if (!sd_fmt) {
972 		if (!dcmi->num_of_sd_formats)
973 			return -ENODATA;
974 
975 		sd_fmt = dcmi->sd_formats[dcmi->num_of_sd_formats - 1];
976 		pix->pixelformat = sd_fmt->fourcc;
977 	}
978 
979 	/* Limit to hardware capabilities */
980 	pix->width = clamp(pix->width, MIN_WIDTH, MAX_WIDTH);
981 	pix->height = clamp(pix->height, MIN_HEIGHT, MAX_HEIGHT);
982 
983 	/* No crop if JPEG is requested */
984 	do_crop = dcmi->do_crop && (pix->pixelformat != V4L2_PIX_FMT_JPEG);
985 
986 	if (do_crop && dcmi->num_of_sd_framesizes) {
987 		struct dcmi_framesize outer_sd_fsize;
988 		/*
989 		 * If crop is requested and sensor have discrete frame sizes,
990 		 * select the frame size that is just larger than request
991 		 */
992 		__find_outer_frame_size(dcmi, pix, &outer_sd_fsize);
993 		pix->width = outer_sd_fsize.width;
994 		pix->height = outer_sd_fsize.height;
995 	}
996 
997 	v4l2_fill_mbus_format(&format.format, pix, sd_fmt->mbus_code);
998 	ret = v4l2_subdev_call_state_try(dcmi->source, pad, set_fmt, &format);
999 	if (ret < 0)
1000 		return ret;
1001 
1002 	/* Update pix regarding to what sensor can do */
1003 	v4l2_fill_pix_format(pix, &format.format);
1004 
1005 	/* Save resolution that sensor can actually do */
1006 	sd_fsize.width = pix->width;
1007 	sd_fsize.height = pix->height;
1008 
1009 	if (do_crop) {
1010 		struct v4l2_rect c = dcmi->crop;
1011 		struct v4l2_rect max_rect;
1012 
1013 		/*
1014 		 * Adjust crop by making the intersection between
1015 		 * format resolution request and crop request
1016 		 */
1017 		max_rect.top = 0;
1018 		max_rect.left = 0;
1019 		max_rect.width = pix->width;
1020 		max_rect.height = pix->height;
1021 		v4l2_rect_map_inside(&c, &max_rect);
1022 		c.top  = clamp_t(s32, c.top, 0, pix->height - c.height);
1023 		c.left = clamp_t(s32, c.left, 0, pix->width - c.width);
1024 		dcmi->crop = c;
1025 
1026 		/* Adjust format resolution request to crop */
1027 		pix->width = dcmi->crop.width;
1028 		pix->height = dcmi->crop.height;
1029 	}
1030 
1031 	pix->field = V4L2_FIELD_NONE;
1032 	pix->bytesperline = pix->width * sd_fmt->bpp;
1033 	pix->sizeimage = pix->bytesperline * pix->height;
1034 
1035 	if (sd_format)
1036 		*sd_format = sd_fmt;
1037 	if (sd_framesize)
1038 		*sd_framesize = sd_fsize;
1039 
1040 	return 0;
1041 }
1042 
1043 static int dcmi_set_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f)
1044 {
1045 	struct v4l2_subdev_format format = {
1046 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
1047 	};
1048 	const struct dcmi_format *sd_format;
1049 	struct dcmi_framesize sd_framesize;
1050 	struct v4l2_mbus_framefmt *mf = &format.format;
1051 	struct v4l2_pix_format *pix = &f->fmt.pix;
1052 	int ret;
1053 
1054 	/*
1055 	 * Try format, fmt.width/height could have been changed
1056 	 * to match sensor capability or crop request
1057 	 * sd_format & sd_framesize will contain what subdev
1058 	 * can do for this request.
1059 	 */
1060 	ret = dcmi_try_fmt(dcmi, f, &sd_format, &sd_framesize);
1061 	if (ret)
1062 		return ret;
1063 
1064 	/* Disable crop if JPEG is requested or BT656 bus is selected */
1065 	if (pix->pixelformat == V4L2_PIX_FMT_JPEG &&
1066 	    dcmi->bus_type != V4L2_MBUS_BT656)
1067 		dcmi->do_crop = false;
1068 
1069 	/* pix to mbus format */
1070 	v4l2_fill_mbus_format(mf, pix,
1071 			      sd_format->mbus_code);
1072 	mf->width = sd_framesize.width;
1073 	mf->height = sd_framesize.height;
1074 
1075 	ret = dcmi_pipeline_s_fmt(dcmi, &format);
1076 	if (ret < 0)
1077 		return ret;
1078 
1079 	dev_dbg(dcmi->dev, "Sensor format set to 0x%x %ux%u\n",
1080 		mf->code, mf->width, mf->height);
1081 	dev_dbg(dcmi->dev, "Buffer format set to %4.4s %ux%u\n",
1082 		(char *)&pix->pixelformat,
1083 		pix->width, pix->height);
1084 
1085 	dcmi->fmt = *f;
1086 	dcmi->sd_format = sd_format;
1087 	dcmi->sd_framesize = sd_framesize;
1088 
1089 	return 0;
1090 }
1091 
1092 static int dcmi_s_fmt_vid_cap(struct file *file, void *priv,
1093 			      struct v4l2_format *f)
1094 {
1095 	struct stm32_dcmi *dcmi = video_drvdata(file);
1096 
1097 	if (vb2_is_streaming(&dcmi->queue))
1098 		return -EBUSY;
1099 
1100 	return dcmi_set_fmt(dcmi, f);
1101 }
1102 
1103 static int dcmi_try_fmt_vid_cap(struct file *file, void *priv,
1104 				struct v4l2_format *f)
1105 {
1106 	struct stm32_dcmi *dcmi = video_drvdata(file);
1107 
1108 	return dcmi_try_fmt(dcmi, f, NULL, NULL);
1109 }
1110 
1111 static int dcmi_enum_fmt_vid_cap(struct file *file, void  *priv,
1112 				 struct v4l2_fmtdesc *f)
1113 {
1114 	struct stm32_dcmi *dcmi = video_drvdata(file);
1115 
1116 	if (f->index >= dcmi->num_of_sd_formats)
1117 		return -EINVAL;
1118 
1119 	f->pixelformat = dcmi->sd_formats[f->index]->fourcc;
1120 	return 0;
1121 }
1122 
1123 static int dcmi_get_sensor_format(struct stm32_dcmi *dcmi,
1124 				  struct v4l2_pix_format *pix)
1125 {
1126 	struct v4l2_subdev_format fmt = {
1127 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
1128 	};
1129 	int ret;
1130 
1131 	ret = v4l2_subdev_call(dcmi->source, pad, get_fmt, NULL, &fmt);
1132 	if (ret)
1133 		return ret;
1134 
1135 	v4l2_fill_pix_format(pix, &fmt.format);
1136 
1137 	return 0;
1138 }
1139 
1140 static int dcmi_set_sensor_format(struct stm32_dcmi *dcmi,
1141 				  struct v4l2_pix_format *pix)
1142 {
1143 	const struct dcmi_format *sd_fmt;
1144 	struct v4l2_subdev_format format = {
1145 		.which = V4L2_SUBDEV_FORMAT_TRY,
1146 	};
1147 	int ret;
1148 
1149 	sd_fmt = find_format_by_fourcc(dcmi, pix->pixelformat);
1150 	if (!sd_fmt) {
1151 		if (!dcmi->num_of_sd_formats)
1152 			return -ENODATA;
1153 
1154 		sd_fmt = dcmi->sd_formats[dcmi->num_of_sd_formats - 1];
1155 		pix->pixelformat = sd_fmt->fourcc;
1156 	}
1157 
1158 	v4l2_fill_mbus_format(&format.format, pix, sd_fmt->mbus_code);
1159 	ret = v4l2_subdev_call_state_try(dcmi->source, pad, set_fmt, &format);
1160 	if (ret < 0)
1161 		return ret;
1162 
1163 	return 0;
1164 }
1165 
1166 static int dcmi_get_sensor_bounds(struct stm32_dcmi *dcmi,
1167 				  struct v4l2_rect *r)
1168 {
1169 	struct v4l2_subdev_selection bounds = {
1170 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
1171 		.target = V4L2_SEL_TGT_CROP_BOUNDS,
1172 	};
1173 	unsigned int max_width, max_height, max_pixsize;
1174 	struct v4l2_pix_format pix;
1175 	unsigned int i;
1176 	int ret;
1177 
1178 	/*
1179 	 * Get sensor bounds first
1180 	 */
1181 	ret = v4l2_subdev_call(dcmi->source, pad, get_selection,
1182 			       NULL, &bounds);
1183 	if (!ret)
1184 		*r = bounds.r;
1185 	if (ret != -ENOIOCTLCMD)
1186 		return ret;
1187 
1188 	/*
1189 	 * If selection is not implemented,
1190 	 * fallback by enumerating sensor frame sizes
1191 	 * and take the largest one
1192 	 */
1193 	max_width = 0;
1194 	max_height = 0;
1195 	max_pixsize = 0;
1196 	for (i = 0; i < dcmi->num_of_sd_framesizes; i++) {
1197 		struct dcmi_framesize *fsize = &dcmi->sd_framesizes[i];
1198 		unsigned int pixsize = fsize->width * fsize->height;
1199 
1200 		if (pixsize > max_pixsize) {
1201 			max_pixsize = pixsize;
1202 			max_width = fsize->width;
1203 			max_height = fsize->height;
1204 		}
1205 	}
1206 	if (max_pixsize > 0) {
1207 		r->top = 0;
1208 		r->left = 0;
1209 		r->width = max_width;
1210 		r->height = max_height;
1211 		return 0;
1212 	}
1213 
1214 	/*
1215 	 * If frame sizes enumeration is not implemented,
1216 	 * fallback by getting current sensor frame size
1217 	 */
1218 	ret = dcmi_get_sensor_format(dcmi, &pix);
1219 	if (ret)
1220 		return ret;
1221 
1222 	r->top = 0;
1223 	r->left = 0;
1224 	r->width = pix.width;
1225 	r->height = pix.height;
1226 
1227 	return 0;
1228 }
1229 
1230 static int dcmi_g_selection(struct file *file, void *fh,
1231 			    struct v4l2_selection *s)
1232 {
1233 	struct stm32_dcmi *dcmi = video_drvdata(file);
1234 
1235 	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1236 		return -EINVAL;
1237 
1238 	switch (s->target) {
1239 	case V4L2_SEL_TGT_CROP_DEFAULT:
1240 	case V4L2_SEL_TGT_CROP_BOUNDS:
1241 		s->r = dcmi->sd_bounds;
1242 		return 0;
1243 	case V4L2_SEL_TGT_CROP:
1244 		if (dcmi->do_crop) {
1245 			s->r = dcmi->crop;
1246 		} else {
1247 			s->r.top = 0;
1248 			s->r.left = 0;
1249 			s->r.width = dcmi->fmt.fmt.pix.width;
1250 			s->r.height = dcmi->fmt.fmt.pix.height;
1251 		}
1252 		break;
1253 	default:
1254 		return -EINVAL;
1255 	}
1256 
1257 	return 0;
1258 }
1259 
1260 static int dcmi_s_selection(struct file *file, void *priv,
1261 			    struct v4l2_selection *s)
1262 {
1263 	struct stm32_dcmi *dcmi = video_drvdata(file);
1264 	struct v4l2_rect r = s->r;
1265 	struct v4l2_rect max_rect;
1266 	struct v4l2_pix_format pix;
1267 
1268 	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
1269 	    s->target != V4L2_SEL_TGT_CROP)
1270 		return -EINVAL;
1271 
1272 	/* Reset sensor resolution to max resolution */
1273 	pix.pixelformat = dcmi->fmt.fmt.pix.pixelformat;
1274 	pix.width = dcmi->sd_bounds.width;
1275 	pix.height = dcmi->sd_bounds.height;
1276 	dcmi_set_sensor_format(dcmi, &pix);
1277 
1278 	/*
1279 	 * Make the intersection between
1280 	 * sensor resolution
1281 	 * and crop request
1282 	 */
1283 	max_rect.top = 0;
1284 	max_rect.left = 0;
1285 	max_rect.width = pix.width;
1286 	max_rect.height = pix.height;
1287 	v4l2_rect_map_inside(&r, &max_rect);
1288 	r.top  = clamp_t(s32, r.top, 0, pix.height - r.height);
1289 	r.left = clamp_t(s32, r.left, 0, pix.width - r.width);
1290 
1291 	if (!(r.top == dcmi->sd_bounds.top &&
1292 	      r.left == dcmi->sd_bounds.left &&
1293 	      r.width == dcmi->sd_bounds.width &&
1294 	      r.height == dcmi->sd_bounds.height)) {
1295 		/* Crop if request is different than sensor resolution */
1296 		dcmi->do_crop = true;
1297 		dcmi->crop = r;
1298 		dev_dbg(dcmi->dev, "s_selection: crop %ux%u@(%u,%u) from %ux%u\n",
1299 			r.width, r.height, r.left, r.top,
1300 			pix.width, pix.height);
1301 	} else {
1302 		/* Disable crop */
1303 		dcmi->do_crop = false;
1304 		dev_dbg(dcmi->dev, "s_selection: crop is disabled\n");
1305 	}
1306 
1307 	s->r = r;
1308 	return 0;
1309 }
1310 
1311 static int dcmi_querycap(struct file *file, void *priv,
1312 			 struct v4l2_capability *cap)
1313 {
1314 	strscpy(cap->driver, DRV_NAME, sizeof(cap->driver));
1315 	strscpy(cap->card, "STM32 Camera Memory Interface",
1316 		sizeof(cap->card));
1317 	strscpy(cap->bus_info, "platform:dcmi", sizeof(cap->bus_info));
1318 	return 0;
1319 }
1320 
1321 static int dcmi_enum_input(struct file *file, void *priv,
1322 			   struct v4l2_input *i)
1323 {
1324 	if (i->index != 0)
1325 		return -EINVAL;
1326 
1327 	i->type = V4L2_INPUT_TYPE_CAMERA;
1328 	strscpy(i->name, "Camera", sizeof(i->name));
1329 	return 0;
1330 }
1331 
1332 static int dcmi_g_input(struct file *file, void *priv, unsigned int *i)
1333 {
1334 	*i = 0;
1335 	return 0;
1336 }
1337 
1338 static int dcmi_s_input(struct file *file, void *priv, unsigned int i)
1339 {
1340 	if (i > 0)
1341 		return -EINVAL;
1342 	return 0;
1343 }
1344 
1345 static int dcmi_enum_framesizes(struct file *file, void *fh,
1346 				struct v4l2_frmsizeenum *fsize)
1347 {
1348 	struct stm32_dcmi *dcmi = video_drvdata(file);
1349 	const struct dcmi_format *sd_fmt;
1350 	struct v4l2_subdev_frame_size_enum fse = {
1351 		.index = fsize->index,
1352 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
1353 	};
1354 	int ret;
1355 
1356 	sd_fmt = find_format_by_fourcc(dcmi, fsize->pixel_format);
1357 	if (!sd_fmt)
1358 		return -EINVAL;
1359 
1360 	fse.code = sd_fmt->mbus_code;
1361 
1362 	ret = v4l2_subdev_call(dcmi->source, pad, enum_frame_size,
1363 			       NULL, &fse);
1364 	if (ret)
1365 		return ret;
1366 
1367 	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
1368 	fsize->discrete.width = fse.max_width;
1369 	fsize->discrete.height = fse.max_height;
1370 
1371 	return 0;
1372 }
1373 
1374 static int dcmi_g_parm(struct file *file, void *priv,
1375 		       struct v4l2_streamparm *p)
1376 {
1377 	struct stm32_dcmi *dcmi = video_drvdata(file);
1378 
1379 	return v4l2_g_parm_cap(video_devdata(file), dcmi->source, p);
1380 }
1381 
1382 static int dcmi_s_parm(struct file *file, void *priv,
1383 		       struct v4l2_streamparm *p)
1384 {
1385 	struct stm32_dcmi *dcmi = video_drvdata(file);
1386 
1387 	return v4l2_s_parm_cap(video_devdata(file), dcmi->source, p);
1388 }
1389 
1390 static int dcmi_enum_frameintervals(struct file *file, void *fh,
1391 				    struct v4l2_frmivalenum *fival)
1392 {
1393 	struct stm32_dcmi *dcmi = video_drvdata(file);
1394 	const struct dcmi_format *sd_fmt;
1395 	struct v4l2_subdev_frame_interval_enum fie = {
1396 		.index = fival->index,
1397 		.width = fival->width,
1398 		.height = fival->height,
1399 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
1400 	};
1401 	int ret;
1402 
1403 	sd_fmt = find_format_by_fourcc(dcmi, fival->pixel_format);
1404 	if (!sd_fmt)
1405 		return -EINVAL;
1406 
1407 	fie.code = sd_fmt->mbus_code;
1408 
1409 	ret = v4l2_subdev_call(dcmi->source, pad,
1410 			       enum_frame_interval, NULL, &fie);
1411 	if (ret)
1412 		return ret;
1413 
1414 	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
1415 	fival->discrete = fie.interval;
1416 
1417 	return 0;
1418 }
1419 
1420 static const struct of_device_id stm32_dcmi_of_match[] = {
1421 	{ .compatible = "st,stm32-dcmi"},
1422 	{ /* end node */ },
1423 };
1424 MODULE_DEVICE_TABLE(of, stm32_dcmi_of_match);
1425 
1426 static int dcmi_open(struct file *file)
1427 {
1428 	struct stm32_dcmi *dcmi = video_drvdata(file);
1429 	struct v4l2_subdev *sd = dcmi->source;
1430 	int ret;
1431 
1432 	if (mutex_lock_interruptible(&dcmi->lock))
1433 		return -ERESTARTSYS;
1434 
1435 	ret = v4l2_fh_open(file);
1436 	if (ret < 0)
1437 		goto unlock;
1438 
1439 	if (!v4l2_fh_is_singular_file(file))
1440 		goto fh_rel;
1441 
1442 	ret = v4l2_subdev_call(sd, core, s_power, 1);
1443 	if (ret < 0 && ret != -ENOIOCTLCMD)
1444 		goto fh_rel;
1445 
1446 	ret = dcmi_set_fmt(dcmi, &dcmi->fmt);
1447 	if (ret)
1448 		v4l2_subdev_call(sd, core, s_power, 0);
1449 fh_rel:
1450 	if (ret)
1451 		v4l2_fh_release(file);
1452 unlock:
1453 	mutex_unlock(&dcmi->lock);
1454 	return ret;
1455 }
1456 
1457 static int dcmi_release(struct file *file)
1458 {
1459 	struct stm32_dcmi *dcmi = video_drvdata(file);
1460 	struct v4l2_subdev *sd = dcmi->source;
1461 	bool fh_singular;
1462 	int ret;
1463 
1464 	mutex_lock(&dcmi->lock);
1465 
1466 	fh_singular = v4l2_fh_is_singular_file(file);
1467 
1468 	ret = _vb2_fop_release(file, NULL);
1469 
1470 	if (fh_singular)
1471 		v4l2_subdev_call(sd, core, s_power, 0);
1472 
1473 	mutex_unlock(&dcmi->lock);
1474 
1475 	return ret;
1476 }
1477 
1478 static const struct v4l2_ioctl_ops dcmi_ioctl_ops = {
1479 	.vidioc_querycap		= dcmi_querycap,
1480 
1481 	.vidioc_try_fmt_vid_cap		= dcmi_try_fmt_vid_cap,
1482 	.vidioc_g_fmt_vid_cap		= dcmi_g_fmt_vid_cap,
1483 	.vidioc_s_fmt_vid_cap		= dcmi_s_fmt_vid_cap,
1484 	.vidioc_enum_fmt_vid_cap	= dcmi_enum_fmt_vid_cap,
1485 	.vidioc_g_selection		= dcmi_g_selection,
1486 	.vidioc_s_selection		= dcmi_s_selection,
1487 
1488 	.vidioc_enum_input		= dcmi_enum_input,
1489 	.vidioc_g_input			= dcmi_g_input,
1490 	.vidioc_s_input			= dcmi_s_input,
1491 
1492 	.vidioc_g_parm			= dcmi_g_parm,
1493 	.vidioc_s_parm			= dcmi_s_parm,
1494 
1495 	.vidioc_enum_framesizes		= dcmi_enum_framesizes,
1496 	.vidioc_enum_frameintervals	= dcmi_enum_frameintervals,
1497 
1498 	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
1499 	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
1500 	.vidioc_querybuf		= vb2_ioctl_querybuf,
1501 	.vidioc_qbuf			= vb2_ioctl_qbuf,
1502 	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
1503 	.vidioc_expbuf			= vb2_ioctl_expbuf,
1504 	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
1505 	.vidioc_streamon		= vb2_ioctl_streamon,
1506 	.vidioc_streamoff		= vb2_ioctl_streamoff,
1507 
1508 	.vidioc_log_status		= v4l2_ctrl_log_status,
1509 	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
1510 	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
1511 };
1512 
1513 static const struct v4l2_file_operations dcmi_fops = {
1514 	.owner		= THIS_MODULE,
1515 	.unlocked_ioctl	= video_ioctl2,
1516 	.open		= dcmi_open,
1517 	.release	= dcmi_release,
1518 	.poll		= vb2_fop_poll,
1519 	.mmap		= vb2_fop_mmap,
1520 #ifndef CONFIG_MMU
1521 	.get_unmapped_area = vb2_fop_get_unmapped_area,
1522 #endif
1523 	.read		= vb2_fop_read,
1524 };
1525 
1526 static int dcmi_set_default_fmt(struct stm32_dcmi *dcmi)
1527 {
1528 	struct v4l2_format f = {
1529 		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
1530 		.fmt.pix = {
1531 			.width		= CIF_WIDTH,
1532 			.height		= CIF_HEIGHT,
1533 			.field		= V4L2_FIELD_NONE,
1534 			.pixelformat	= dcmi->sd_formats[0]->fourcc,
1535 		},
1536 	};
1537 	int ret;
1538 
1539 	ret = dcmi_try_fmt(dcmi, &f, NULL, NULL);
1540 	if (ret)
1541 		return ret;
1542 	dcmi->sd_format = dcmi->sd_formats[0];
1543 	dcmi->fmt = f;
1544 	return 0;
1545 }
1546 
1547 static const struct dcmi_format dcmi_formats[] = {
1548 	{
1549 		.fourcc = V4L2_PIX_FMT_RGB565,
1550 		.mbus_code = MEDIA_BUS_FMT_RGB565_2X8_LE,
1551 		.bpp = 2,
1552 	}, {
1553 		.fourcc = V4L2_PIX_FMT_RGB565,
1554 		.mbus_code = MEDIA_BUS_FMT_RGB565_1X16,
1555 		.bpp = 2,
1556 	}, {
1557 		.fourcc = V4L2_PIX_FMT_YUYV,
1558 		.mbus_code = MEDIA_BUS_FMT_YUYV8_2X8,
1559 		.bpp = 2,
1560 	}, {
1561 		.fourcc = V4L2_PIX_FMT_YUYV,
1562 		.mbus_code = MEDIA_BUS_FMT_YUYV8_1X16,
1563 		.bpp = 2,
1564 	}, {
1565 		.fourcc = V4L2_PIX_FMT_UYVY,
1566 		.mbus_code = MEDIA_BUS_FMT_UYVY8_2X8,
1567 		.bpp = 2,
1568 	}, {
1569 		.fourcc = V4L2_PIX_FMT_UYVY,
1570 		.mbus_code = MEDIA_BUS_FMT_UYVY8_1X16,
1571 		.bpp = 2,
1572 	}, {
1573 		.fourcc = V4L2_PIX_FMT_JPEG,
1574 		.mbus_code = MEDIA_BUS_FMT_JPEG_1X8,
1575 		.bpp = 1,
1576 	}, {
1577 		.fourcc = V4L2_PIX_FMT_SBGGR8,
1578 		.mbus_code = MEDIA_BUS_FMT_SBGGR8_1X8,
1579 		.bpp = 1,
1580 	}, {
1581 		.fourcc = V4L2_PIX_FMT_SGBRG8,
1582 		.mbus_code = MEDIA_BUS_FMT_SGBRG8_1X8,
1583 		.bpp = 1,
1584 	}, {
1585 		.fourcc = V4L2_PIX_FMT_SGRBG8,
1586 		.mbus_code = MEDIA_BUS_FMT_SGRBG8_1X8,
1587 		.bpp = 1,
1588 	}, {
1589 		.fourcc = V4L2_PIX_FMT_SRGGB8,
1590 		.mbus_code = MEDIA_BUS_FMT_SRGGB8_1X8,
1591 		.bpp = 1,
1592 	}, {
1593 		.fourcc = V4L2_PIX_FMT_SBGGR10,
1594 		.mbus_code = MEDIA_BUS_FMT_SBGGR10_1X10,
1595 		.bpp = 2,
1596 	}, {
1597 		.fourcc = V4L2_PIX_FMT_SGBRG10,
1598 		.mbus_code = MEDIA_BUS_FMT_SGBRG10_1X10,
1599 		.bpp = 2,
1600 	}, {
1601 		.fourcc = V4L2_PIX_FMT_SGRBG10,
1602 		.mbus_code = MEDIA_BUS_FMT_SGRBG10_1X10,
1603 		.bpp = 2,
1604 	}, {
1605 		.fourcc = V4L2_PIX_FMT_SRGGB10,
1606 		.mbus_code = MEDIA_BUS_FMT_SRGGB10_1X10,
1607 		.bpp = 2,
1608 	}, {
1609 		.fourcc = V4L2_PIX_FMT_SBGGR12,
1610 		.mbus_code = MEDIA_BUS_FMT_SBGGR12_1X12,
1611 		.bpp = 2,
1612 	}, {
1613 		.fourcc = V4L2_PIX_FMT_SGBRG12,
1614 		.mbus_code = MEDIA_BUS_FMT_SGBRG12_1X12,
1615 		.bpp = 2,
1616 	}, {
1617 		.fourcc = V4L2_PIX_FMT_SGRBG12,
1618 		.mbus_code = MEDIA_BUS_FMT_SGRBG12_1X12,
1619 		.bpp = 2,
1620 	}, {
1621 		.fourcc = V4L2_PIX_FMT_SRGGB12,
1622 		.mbus_code = MEDIA_BUS_FMT_SRGGB12_1X12,
1623 		.bpp = 2,
1624 	}, {
1625 		.fourcc = V4L2_PIX_FMT_SBGGR14,
1626 		.mbus_code = MEDIA_BUS_FMT_SBGGR14_1X14,
1627 		.bpp = 2,
1628 	}, {
1629 		.fourcc = V4L2_PIX_FMT_SGBRG14,
1630 		.mbus_code = MEDIA_BUS_FMT_SGBRG14_1X14,
1631 		.bpp = 2,
1632 	}, {
1633 		.fourcc = V4L2_PIX_FMT_SGRBG14,
1634 		.mbus_code = MEDIA_BUS_FMT_SGRBG14_1X14,
1635 		.bpp = 2,
1636 	}, {
1637 		.fourcc = V4L2_PIX_FMT_SRGGB14,
1638 		.mbus_code = MEDIA_BUS_FMT_SRGGB14_1X14,
1639 		.bpp = 2,
1640 	},
1641 };
1642 
1643 static int dcmi_formats_init(struct stm32_dcmi *dcmi)
1644 {
1645 	const struct dcmi_format *sd_fmts[ARRAY_SIZE(dcmi_formats)];
1646 	unsigned int num_fmts = 0, i, j;
1647 	struct v4l2_subdev *subdev = dcmi->source;
1648 	struct v4l2_subdev_mbus_code_enum mbus_code = {
1649 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
1650 	};
1651 
1652 	while (!v4l2_subdev_call(subdev, pad, enum_mbus_code,
1653 				 NULL, &mbus_code)) {
1654 		for (i = 0; i < ARRAY_SIZE(dcmi_formats); i++) {
1655 			if (dcmi_formats[i].mbus_code != mbus_code.code)
1656 				continue;
1657 
1658 			/* Exclude JPEG if BT656 bus is selected */
1659 			if (dcmi_formats[i].fourcc == V4L2_PIX_FMT_JPEG &&
1660 			    dcmi->bus_type == V4L2_MBUS_BT656)
1661 				continue;
1662 
1663 			/* Code supported, have we got this fourcc yet? */
1664 			for (j = 0; j < num_fmts; j++)
1665 				if (sd_fmts[j]->fourcc ==
1666 						dcmi_formats[i].fourcc) {
1667 					/* Already available */
1668 					dev_dbg(dcmi->dev, "Skipping fourcc/code: %4.4s/0x%x\n",
1669 						(char *)&sd_fmts[j]->fourcc,
1670 						mbus_code.code);
1671 					break;
1672 				}
1673 			if (j == num_fmts) {
1674 				/* New */
1675 				sd_fmts[num_fmts++] = dcmi_formats + i;
1676 				dev_dbg(dcmi->dev, "Supported fourcc/code: %4.4s/0x%x\n",
1677 					(char *)&sd_fmts[num_fmts - 1]->fourcc,
1678 					sd_fmts[num_fmts - 1]->mbus_code);
1679 			}
1680 		}
1681 		mbus_code.index++;
1682 	}
1683 
1684 	if (!num_fmts)
1685 		return -ENXIO;
1686 
1687 	dcmi->num_of_sd_formats = num_fmts;
1688 	dcmi->sd_formats = devm_kcalloc(dcmi->dev,
1689 					num_fmts, sizeof(struct dcmi_format *),
1690 					GFP_KERNEL);
1691 	if (!dcmi->sd_formats) {
1692 		dev_err(dcmi->dev, "Could not allocate memory\n");
1693 		return -ENOMEM;
1694 	}
1695 
1696 	memcpy(dcmi->sd_formats, sd_fmts,
1697 	       num_fmts * sizeof(struct dcmi_format *));
1698 	dcmi->sd_format = dcmi->sd_formats[0];
1699 
1700 	return 0;
1701 }
1702 
1703 static int dcmi_framesizes_init(struct stm32_dcmi *dcmi)
1704 {
1705 	unsigned int num_fsize = 0;
1706 	struct v4l2_subdev *subdev = dcmi->source;
1707 	struct v4l2_subdev_frame_size_enum fse = {
1708 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
1709 		.code = dcmi->sd_format->mbus_code,
1710 	};
1711 	unsigned int ret;
1712 	unsigned int i;
1713 
1714 	/* Allocate discrete framesizes array */
1715 	while (!v4l2_subdev_call(subdev, pad, enum_frame_size,
1716 				 NULL, &fse))
1717 		fse.index++;
1718 
1719 	num_fsize = fse.index;
1720 	if (!num_fsize)
1721 		return 0;
1722 
1723 	dcmi->num_of_sd_framesizes = num_fsize;
1724 	dcmi->sd_framesizes = devm_kcalloc(dcmi->dev, num_fsize,
1725 					   sizeof(struct dcmi_framesize),
1726 					   GFP_KERNEL);
1727 	if (!dcmi->sd_framesizes) {
1728 		dev_err(dcmi->dev, "Could not allocate memory\n");
1729 		return -ENOMEM;
1730 	}
1731 
1732 	/* Fill array with sensor supported framesizes */
1733 	dev_dbg(dcmi->dev, "Sensor supports %u frame sizes:\n", num_fsize);
1734 	for (i = 0; i < dcmi->num_of_sd_framesizes; i++) {
1735 		fse.index = i;
1736 		ret = v4l2_subdev_call(subdev, pad, enum_frame_size,
1737 				       NULL, &fse);
1738 		if (ret)
1739 			return ret;
1740 		dcmi->sd_framesizes[fse.index].width = fse.max_width;
1741 		dcmi->sd_framesizes[fse.index].height = fse.max_height;
1742 		dev_dbg(dcmi->dev, "%ux%u\n", fse.max_width, fse.max_height);
1743 	}
1744 
1745 	return 0;
1746 }
1747 
1748 static int dcmi_graph_notify_complete(struct v4l2_async_notifier *notifier)
1749 {
1750 	struct stm32_dcmi *dcmi = notifier_to_dcmi(notifier);
1751 	int ret;
1752 
1753 	/*
1754 	 * Now that the graph is complete,
1755 	 * we search for the source subdevice
1756 	 * in order to expose it through V4L2 interface
1757 	 */
1758 	dcmi->source = media_entity_to_v4l2_subdev(dcmi_find_source(dcmi));
1759 	if (!dcmi->source) {
1760 		dev_err(dcmi->dev, "Source subdevice not found\n");
1761 		return -ENODEV;
1762 	}
1763 
1764 	dcmi->vdev->ctrl_handler = dcmi->source->ctrl_handler;
1765 
1766 	ret = dcmi_formats_init(dcmi);
1767 	if (ret) {
1768 		dev_err(dcmi->dev, "No supported mediabus format found\n");
1769 		return ret;
1770 	}
1771 
1772 	ret = dcmi_framesizes_init(dcmi);
1773 	if (ret) {
1774 		dev_err(dcmi->dev, "Could not initialize framesizes\n");
1775 		return ret;
1776 	}
1777 
1778 	ret = dcmi_get_sensor_bounds(dcmi, &dcmi->sd_bounds);
1779 	if (ret) {
1780 		dev_err(dcmi->dev, "Could not get sensor bounds\n");
1781 		return ret;
1782 	}
1783 
1784 	ret = dcmi_set_default_fmt(dcmi);
1785 	if (ret) {
1786 		dev_err(dcmi->dev, "Could not set default format\n");
1787 		return ret;
1788 	}
1789 
1790 	ret = devm_request_threaded_irq(dcmi->dev, dcmi->irq, dcmi_irq_callback,
1791 					dcmi_irq_thread, IRQF_ONESHOT,
1792 					dev_name(dcmi->dev), dcmi);
1793 	if (ret) {
1794 		dev_err(dcmi->dev, "Unable to request irq %d\n", dcmi->irq);
1795 		return ret;
1796 	}
1797 
1798 	return 0;
1799 }
1800 
1801 static void dcmi_graph_notify_unbind(struct v4l2_async_notifier *notifier,
1802 				     struct v4l2_subdev *sd,
1803 				     struct v4l2_async_connection *asd)
1804 {
1805 	struct stm32_dcmi *dcmi = notifier_to_dcmi(notifier);
1806 
1807 	dev_dbg(dcmi->dev, "Removing %s\n", video_device_node_name(dcmi->vdev));
1808 
1809 	/* Checks internally if vdev has been init or not */
1810 	video_unregister_device(dcmi->vdev);
1811 }
1812 
1813 static int dcmi_graph_notify_bound(struct v4l2_async_notifier *notifier,
1814 				   struct v4l2_subdev *subdev,
1815 				   struct v4l2_async_connection *asd)
1816 {
1817 	struct stm32_dcmi *dcmi = notifier_to_dcmi(notifier);
1818 	unsigned int ret;
1819 	int src_pad;
1820 
1821 	dev_dbg(dcmi->dev, "Subdev \"%s\" bound\n", subdev->name);
1822 
1823 	/*
1824 	 * Link this sub-device to DCMI, it could be
1825 	 * a parallel camera sensor or a bridge
1826 	 */
1827 	src_pad = media_entity_get_fwnode_pad(&subdev->entity,
1828 					      subdev->fwnode,
1829 					      MEDIA_PAD_FL_SOURCE);
1830 
1831 	ret = media_create_pad_link(&subdev->entity, src_pad,
1832 				    &dcmi->vdev->entity, 0,
1833 				    MEDIA_LNK_FL_IMMUTABLE |
1834 				    MEDIA_LNK_FL_ENABLED);
1835 	if (ret)
1836 		dev_err(dcmi->dev, "Failed to create media pad link with subdev \"%s\"\n",
1837 			subdev->name);
1838 	else
1839 		dev_dbg(dcmi->dev, "DCMI is now linked to \"%s\"\n",
1840 			subdev->name);
1841 
1842 	dcmi->s_subdev = subdev;
1843 
1844 	return ret;
1845 }
1846 
1847 static const struct v4l2_async_notifier_operations dcmi_graph_notify_ops = {
1848 	.bound = dcmi_graph_notify_bound,
1849 	.unbind = dcmi_graph_notify_unbind,
1850 	.complete = dcmi_graph_notify_complete,
1851 };
1852 
1853 static int dcmi_graph_init(struct stm32_dcmi *dcmi)
1854 {
1855 	struct v4l2_async_connection *asd;
1856 	struct device_node *ep;
1857 	int ret;
1858 
1859 	ep = of_graph_get_next_endpoint(dcmi->dev->of_node, NULL);
1860 	if (!ep) {
1861 		dev_err(dcmi->dev, "Failed to get next endpoint\n");
1862 		return -EINVAL;
1863 	}
1864 
1865 	v4l2_async_nf_init(&dcmi->notifier, &dcmi->v4l2_dev);
1866 
1867 	asd = v4l2_async_nf_add_fwnode_remote(&dcmi->notifier,
1868 					      of_fwnode_handle(ep),
1869 					      struct v4l2_async_connection);
1870 
1871 	of_node_put(ep);
1872 
1873 	if (IS_ERR(asd)) {
1874 		dev_err(dcmi->dev, "Failed to add subdev notifier\n");
1875 		return PTR_ERR(asd);
1876 	}
1877 
1878 	dcmi->notifier.ops = &dcmi_graph_notify_ops;
1879 
1880 	ret = v4l2_async_nf_register(&dcmi->notifier);
1881 	if (ret < 0) {
1882 		dev_err(dcmi->dev, "Failed to register notifier\n");
1883 		v4l2_async_nf_cleanup(&dcmi->notifier);
1884 		return ret;
1885 	}
1886 
1887 	return 0;
1888 }
1889 
1890 static int dcmi_probe(struct platform_device *pdev)
1891 {
1892 	struct device_node *np = pdev->dev.of_node;
1893 	const struct of_device_id *match = NULL;
1894 	struct v4l2_fwnode_endpoint ep = { .bus_type = 0 };
1895 	struct stm32_dcmi *dcmi;
1896 	struct vb2_queue *q;
1897 	struct dma_chan *chan;
1898 	struct dma_slave_caps caps;
1899 	struct clk *mclk;
1900 	int ret = 0;
1901 
1902 	match = of_match_device(of_match_ptr(stm32_dcmi_of_match), &pdev->dev);
1903 	if (!match) {
1904 		dev_err(&pdev->dev, "Could not find a match in devicetree\n");
1905 		return -ENODEV;
1906 	}
1907 
1908 	dcmi = devm_kzalloc(&pdev->dev, sizeof(struct stm32_dcmi), GFP_KERNEL);
1909 	if (!dcmi)
1910 		return -ENOMEM;
1911 
1912 	dcmi->rstc = devm_reset_control_get_exclusive(&pdev->dev, NULL);
1913 	if (IS_ERR(dcmi->rstc))
1914 		return dev_err_probe(&pdev->dev, PTR_ERR(dcmi->rstc),
1915 				     "Could not get reset control\n");
1916 
1917 	/* Get bus characteristics from devicetree */
1918 	np = of_graph_get_next_endpoint(np, NULL);
1919 	if (!np) {
1920 		dev_err(&pdev->dev, "Could not find the endpoint\n");
1921 		return -ENODEV;
1922 	}
1923 
1924 	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(np), &ep);
1925 	of_node_put(np);
1926 	if (ret) {
1927 		dev_err(&pdev->dev, "Could not parse the endpoint\n");
1928 		return ret;
1929 	}
1930 
1931 	if (ep.bus_type == V4L2_MBUS_CSI2_DPHY) {
1932 		dev_err(&pdev->dev, "CSI bus not supported\n");
1933 		return -ENODEV;
1934 	}
1935 
1936 	if (ep.bus_type == V4L2_MBUS_BT656 &&
1937 	    ep.bus.parallel.bus_width != 8) {
1938 		dev_err(&pdev->dev, "BT656 bus conflicts with %u bits bus width (8 bits required)\n",
1939 			ep.bus.parallel.bus_width);
1940 		return -ENODEV;
1941 	}
1942 
1943 	dcmi->bus.flags = ep.bus.parallel.flags;
1944 	dcmi->bus.bus_width = ep.bus.parallel.bus_width;
1945 	dcmi->bus.data_shift = ep.bus.parallel.data_shift;
1946 	dcmi->bus_type = ep.bus_type;
1947 
1948 	dcmi->irq = platform_get_irq(pdev, 0);
1949 	if (dcmi->irq < 0)
1950 		return dcmi->irq;
1951 
1952 	dcmi->regs = devm_platform_get_and_ioremap_resource(pdev, 0, &dcmi->res);
1953 	if (IS_ERR(dcmi->regs))
1954 		return PTR_ERR(dcmi->regs);
1955 
1956 	mclk = devm_clk_get(&pdev->dev, "mclk");
1957 	if (IS_ERR(mclk))
1958 		return dev_err_probe(&pdev->dev, PTR_ERR(mclk),
1959 				     "Unable to get mclk\n");
1960 
1961 	chan = dma_request_chan(&pdev->dev, "tx");
1962 	if (IS_ERR(chan))
1963 		return dev_err_probe(&pdev->dev, PTR_ERR(chan),
1964 				     "Failed to request DMA channel\n");
1965 
1966 	dcmi->dma_max_burst = UINT_MAX;
1967 	ret = dma_get_slave_caps(chan, &caps);
1968 	if (!ret && caps.max_sg_burst)
1969 		dcmi->dma_max_burst = caps.max_sg_burst	* DMA_SLAVE_BUSWIDTH_4_BYTES;
1970 
1971 	spin_lock_init(&dcmi->irqlock);
1972 	mutex_init(&dcmi->lock);
1973 	mutex_init(&dcmi->dma_lock);
1974 	init_completion(&dcmi->complete);
1975 	INIT_LIST_HEAD(&dcmi->buffers);
1976 
1977 	dcmi->dev = &pdev->dev;
1978 	dcmi->mclk = mclk;
1979 	dcmi->state = STOPPED;
1980 	dcmi->dma_chan = chan;
1981 
1982 	q = &dcmi->queue;
1983 
1984 	dcmi->v4l2_dev.mdev = &dcmi->mdev;
1985 
1986 	/* Initialize media device */
1987 	strscpy(dcmi->mdev.model, DRV_NAME, sizeof(dcmi->mdev.model));
1988 	dcmi->mdev.dev = &pdev->dev;
1989 	media_device_init(&dcmi->mdev);
1990 
1991 	/* Initialize the top-level structure */
1992 	ret = v4l2_device_register(&pdev->dev, &dcmi->v4l2_dev);
1993 	if (ret)
1994 		goto err_media_device_cleanup;
1995 
1996 	dcmi->vdev = video_device_alloc();
1997 	if (!dcmi->vdev) {
1998 		ret = -ENOMEM;
1999 		goto err_device_unregister;
2000 	}
2001 
2002 	/* Video node */
2003 	dcmi->vdev->fops = &dcmi_fops;
2004 	dcmi->vdev->v4l2_dev = &dcmi->v4l2_dev;
2005 	dcmi->vdev->queue = &dcmi->queue;
2006 	strscpy(dcmi->vdev->name, KBUILD_MODNAME, sizeof(dcmi->vdev->name));
2007 	dcmi->vdev->release = video_device_release;
2008 	dcmi->vdev->ioctl_ops = &dcmi_ioctl_ops;
2009 	dcmi->vdev->lock = &dcmi->lock;
2010 	dcmi->vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
2011 				  V4L2_CAP_READWRITE;
2012 	video_set_drvdata(dcmi->vdev, dcmi);
2013 
2014 	/* Media entity pads */
2015 	dcmi->vid_cap_pad.flags = MEDIA_PAD_FL_SINK;
2016 	ret = media_entity_pads_init(&dcmi->vdev->entity,
2017 				     1, &dcmi->vid_cap_pad);
2018 	if (ret) {
2019 		dev_err(dcmi->dev, "Failed to init media entity pad\n");
2020 		goto err_device_release;
2021 	}
2022 	dcmi->vdev->entity.flags |= MEDIA_ENT_FL_DEFAULT;
2023 
2024 	ret = video_register_device(dcmi->vdev, VFL_TYPE_VIDEO, -1);
2025 	if (ret) {
2026 		dev_err(dcmi->dev, "Failed to register video device\n");
2027 		goto err_media_entity_cleanup;
2028 	}
2029 
2030 	dev_dbg(dcmi->dev, "Device registered as %s\n",
2031 		video_device_node_name(dcmi->vdev));
2032 
2033 	/* Buffer queue */
2034 	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
2035 	q->io_modes = VB2_MMAP | VB2_READ | VB2_DMABUF;
2036 	q->lock = &dcmi->lock;
2037 	q->drv_priv = dcmi;
2038 	q->buf_struct_size = sizeof(struct dcmi_buf);
2039 	q->ops = &dcmi_video_qops;
2040 	q->mem_ops = &vb2_dma_contig_memops;
2041 	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
2042 	q->min_buffers_needed = 2;
2043 	q->allow_cache_hints = 1;
2044 	q->dev = &pdev->dev;
2045 
2046 	ret = vb2_queue_init(q);
2047 	if (ret < 0) {
2048 		dev_err(&pdev->dev, "Failed to initialize vb2 queue\n");
2049 		goto err_media_entity_cleanup;
2050 	}
2051 
2052 	ret = dcmi_graph_init(dcmi);
2053 	if (ret < 0)
2054 		goto err_media_entity_cleanup;
2055 
2056 	/* Reset device */
2057 	ret = reset_control_assert(dcmi->rstc);
2058 	if (ret) {
2059 		dev_err(&pdev->dev, "Failed to assert the reset line\n");
2060 		goto err_cleanup;
2061 	}
2062 
2063 	usleep_range(3000, 5000);
2064 
2065 	ret = reset_control_deassert(dcmi->rstc);
2066 	if (ret) {
2067 		dev_err(&pdev->dev, "Failed to deassert the reset line\n");
2068 		goto err_cleanup;
2069 	}
2070 
2071 	dev_info(&pdev->dev, "Probe done\n");
2072 
2073 	platform_set_drvdata(pdev, dcmi);
2074 
2075 	pm_runtime_enable(&pdev->dev);
2076 
2077 	return 0;
2078 
2079 err_cleanup:
2080 	v4l2_async_nf_cleanup(&dcmi->notifier);
2081 err_media_entity_cleanup:
2082 	media_entity_cleanup(&dcmi->vdev->entity);
2083 err_device_release:
2084 	video_device_release(dcmi->vdev);
2085 err_device_unregister:
2086 	v4l2_device_unregister(&dcmi->v4l2_dev);
2087 err_media_device_cleanup:
2088 	media_device_cleanup(&dcmi->mdev);
2089 	dma_release_channel(dcmi->dma_chan);
2090 
2091 	return ret;
2092 }
2093 
2094 static void dcmi_remove(struct platform_device *pdev)
2095 {
2096 	struct stm32_dcmi *dcmi = platform_get_drvdata(pdev);
2097 
2098 	pm_runtime_disable(&pdev->dev);
2099 
2100 	v4l2_async_nf_unregister(&dcmi->notifier);
2101 	v4l2_async_nf_cleanup(&dcmi->notifier);
2102 	media_entity_cleanup(&dcmi->vdev->entity);
2103 	v4l2_device_unregister(&dcmi->v4l2_dev);
2104 	media_device_cleanup(&dcmi->mdev);
2105 
2106 	dma_release_channel(dcmi->dma_chan);
2107 }
2108 
2109 static __maybe_unused int dcmi_runtime_suspend(struct device *dev)
2110 {
2111 	struct stm32_dcmi *dcmi = dev_get_drvdata(dev);
2112 
2113 	clk_disable_unprepare(dcmi->mclk);
2114 
2115 	return 0;
2116 }
2117 
2118 static __maybe_unused int dcmi_runtime_resume(struct device *dev)
2119 {
2120 	struct stm32_dcmi *dcmi = dev_get_drvdata(dev);
2121 	int ret;
2122 
2123 	ret = clk_prepare_enable(dcmi->mclk);
2124 	if (ret)
2125 		dev_err(dev, "%s: Failed to prepare_enable clock\n", __func__);
2126 
2127 	return ret;
2128 }
2129 
2130 static __maybe_unused int dcmi_suspend(struct device *dev)
2131 {
2132 	/* disable clock */
2133 	pm_runtime_force_suspend(dev);
2134 
2135 	/* change pinctrl state */
2136 	pinctrl_pm_select_sleep_state(dev);
2137 
2138 	return 0;
2139 }
2140 
2141 static __maybe_unused int dcmi_resume(struct device *dev)
2142 {
2143 	/* restore pinctl default state */
2144 	pinctrl_pm_select_default_state(dev);
2145 
2146 	/* clock enable */
2147 	pm_runtime_force_resume(dev);
2148 
2149 	return 0;
2150 }
2151 
2152 static const struct dev_pm_ops dcmi_pm_ops = {
2153 	SET_SYSTEM_SLEEP_PM_OPS(dcmi_suspend, dcmi_resume)
2154 	SET_RUNTIME_PM_OPS(dcmi_runtime_suspend,
2155 			   dcmi_runtime_resume, NULL)
2156 };
2157 
2158 static struct platform_driver stm32_dcmi_driver = {
2159 	.probe		= dcmi_probe,
2160 	.remove_new	= dcmi_remove,
2161 	.driver		= {
2162 		.name = DRV_NAME,
2163 		.of_match_table = of_match_ptr(stm32_dcmi_of_match),
2164 		.pm = &dcmi_pm_ops,
2165 	},
2166 };
2167 
2168 module_platform_driver(stm32_dcmi_driver);
2169 
2170 MODULE_AUTHOR("Yannick Fertre <yannick.fertre@st.com>");
2171 MODULE_AUTHOR("Hugues Fruchet <hugues.fruchet@st.com>");
2172 MODULE_DESCRIPTION("STMicroelectronics STM32 Digital Camera Memory Interface driver");
2173 MODULE_LICENSE("GPL");
2174