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