1 /*
2  * Copyright (c) 2011 Atmel Corporation
3  * Josh Wu, <josh.wu@atmel.com>
4  *
5  * Based on previous work by Lars Haring, <lars.haring@atmel.com>
6  * and Sedji Gaouaou
7  * Based on the bttv driver for Bt848 with respective copyright holders
8  *
9  * This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License version 2 as
11  * published by the Free Software Foundation.
12  */
13 
14 #include <linux/clk.h>
15 #include <linux/completion.h>
16 #include <linux/delay.h>
17 #include <linux/fs.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_graph.h>
23 #include <linux/platform_device.h>
24 #include <linux/pm_runtime.h>
25 #include <linux/slab.h>
26 #include <linux/of.h>
27 
28 #include <linux/videodev2.h>
29 #include <media/v4l2-ctrls.h>
30 #include <media/v4l2-device.h>
31 #include <media/v4l2-dev.h>
32 #include <media/v4l2-ioctl.h>
33 #include <media/v4l2-event.h>
34 #include <media/v4l2-fwnode.h>
35 #include <media/videobuf2-dma-contig.h>
36 #include <media/v4l2-image-sizes.h>
37 
38 #include "atmel-isi.h"
39 
40 #define MAX_SUPPORT_WIDTH		2048U
41 #define MAX_SUPPORT_HEIGHT		2048U
42 #define MIN_FRAME_RATE			15
43 #define FRAME_INTERVAL_MILLI_SEC	(1000 / MIN_FRAME_RATE)
44 
45 /* Frame buffer descriptor */
46 struct fbd {
47 	/* Physical address of the frame buffer */
48 	u32 fb_address;
49 	/* DMA Control Register(only in HISI2) */
50 	u32 dma_ctrl;
51 	/* Physical address of the next fbd */
52 	u32 next_fbd_address;
53 };
54 
55 static void set_dma_ctrl(struct fbd *fb_desc, u32 ctrl)
56 {
57 	fb_desc->dma_ctrl = ctrl;
58 }
59 
60 struct isi_dma_desc {
61 	struct list_head list;
62 	struct fbd *p_fbd;
63 	dma_addr_t fbd_phys;
64 };
65 
66 /* Frame buffer data */
67 struct frame_buffer {
68 	struct vb2_v4l2_buffer vb;
69 	struct isi_dma_desc *p_dma_desc;
70 	struct list_head list;
71 };
72 
73 struct isi_graph_entity {
74 	struct device_node *node;
75 
76 	struct v4l2_async_subdev asd;
77 	struct v4l2_subdev *subdev;
78 };
79 
80 /*
81  * struct isi_format - ISI media bus format information
82  * @fourcc:		Fourcc code for this format
83  * @mbus_code:		V4L2 media bus format code.
84  * @bpp:		Bytes per pixel (when stored in memory)
85  * @swap:		Byte swap configuration value
86  * @support:		Indicates format supported by subdev
87  * @skip:		Skip duplicate format supported by subdev
88  */
89 struct isi_format {
90 	u32	fourcc;
91 	u32	mbus_code;
92 	u8	bpp;
93 	u32	swap;
94 };
95 
96 
97 struct atmel_isi {
98 	/* Protects the access of variables shared with the ISR */
99 	spinlock_t			irqlock;
100 	struct device			*dev;
101 	void __iomem			*regs;
102 
103 	int				sequence;
104 
105 	/* Allocate descriptors for dma buffer use */
106 	struct fbd			*p_fb_descriptors;
107 	dma_addr_t			fb_descriptors_phys;
108 	struct				list_head dma_desc_head;
109 	struct isi_dma_desc		dma_desc[VIDEO_MAX_FRAME];
110 	bool				enable_preview_path;
111 
112 	struct completion		complete;
113 	/* ISI peripherial clock */
114 	struct clk			*pclk;
115 	unsigned int			irq;
116 
117 	struct isi_platform_data	pdata;
118 	u16				width_flags;	/* max 12 bits */
119 
120 	struct list_head		video_buffer_list;
121 	struct frame_buffer		*active;
122 
123 	struct v4l2_device		v4l2_dev;
124 	struct video_device		*vdev;
125 	struct v4l2_async_notifier	notifier;
126 	struct isi_graph_entity		entity;
127 	struct v4l2_format		fmt;
128 
129 	const struct isi_format		**user_formats;
130 	unsigned int			num_user_formats;
131 	const struct isi_format		*current_fmt;
132 
133 	struct mutex			lock;
134 	struct vb2_queue		queue;
135 };
136 
137 #define notifier_to_isi(n) container_of(n, struct atmel_isi, notifier)
138 
139 static void isi_writel(struct atmel_isi *isi, u32 reg, u32 val)
140 {
141 	writel(val, isi->regs + reg);
142 }
143 static u32 isi_readl(struct atmel_isi *isi, u32 reg)
144 {
145 	return readl(isi->regs + reg);
146 }
147 
148 static void configure_geometry(struct atmel_isi *isi)
149 {
150 	u32 cfg2, psize;
151 	u32 fourcc = isi->current_fmt->fourcc;
152 
153 	isi->enable_preview_path = fourcc == V4L2_PIX_FMT_RGB565 ||
154 				   fourcc == V4L2_PIX_FMT_RGB32;
155 
156 	/* According to sensor's output format to set cfg2 */
157 	cfg2 = isi->current_fmt->swap;
158 
159 	isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
160 	/* Set width */
161 	cfg2 |= ((isi->fmt.fmt.pix.width - 1) << ISI_CFG2_IM_HSIZE_OFFSET) &
162 			ISI_CFG2_IM_HSIZE_MASK;
163 	/* Set height */
164 	cfg2 |= ((isi->fmt.fmt.pix.height - 1) << ISI_CFG2_IM_VSIZE_OFFSET)
165 			& ISI_CFG2_IM_VSIZE_MASK;
166 	isi_writel(isi, ISI_CFG2, cfg2);
167 
168 	/* No down sampling, preview size equal to sensor output size */
169 	psize = ((isi->fmt.fmt.pix.width - 1) << ISI_PSIZE_PREV_HSIZE_OFFSET) &
170 		ISI_PSIZE_PREV_HSIZE_MASK;
171 	psize |= ((isi->fmt.fmt.pix.height - 1) << ISI_PSIZE_PREV_VSIZE_OFFSET) &
172 		ISI_PSIZE_PREV_VSIZE_MASK;
173 	isi_writel(isi, ISI_PSIZE, psize);
174 	isi_writel(isi, ISI_PDECF, ISI_PDECF_NO_SAMPLING);
175 }
176 
177 static irqreturn_t atmel_isi_handle_streaming(struct atmel_isi *isi)
178 {
179 	if (isi->active) {
180 		struct vb2_v4l2_buffer *vbuf = &isi->active->vb;
181 		struct frame_buffer *buf = isi->active;
182 
183 		list_del_init(&buf->list);
184 		vbuf->vb2_buf.timestamp = ktime_get_ns();
185 		vbuf->sequence = isi->sequence++;
186 		vbuf->field = V4L2_FIELD_NONE;
187 		vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_DONE);
188 	}
189 
190 	if (list_empty(&isi->video_buffer_list)) {
191 		isi->active = NULL;
192 	} else {
193 		/* start next dma frame. */
194 		isi->active = list_entry(isi->video_buffer_list.next,
195 					struct frame_buffer, list);
196 		if (!isi->enable_preview_path) {
197 			isi_writel(isi, ISI_DMA_C_DSCR,
198 				(u32)isi->active->p_dma_desc->fbd_phys);
199 			isi_writel(isi, ISI_DMA_C_CTRL,
200 				ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
201 			isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_C_CH);
202 		} else {
203 			isi_writel(isi, ISI_DMA_P_DSCR,
204 				(u32)isi->active->p_dma_desc->fbd_phys);
205 			isi_writel(isi, ISI_DMA_P_CTRL,
206 				ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
207 			isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_P_CH);
208 		}
209 	}
210 	return IRQ_HANDLED;
211 }
212 
213 /* ISI interrupt service routine */
214 static irqreturn_t isi_interrupt(int irq, void *dev_id)
215 {
216 	struct atmel_isi *isi = dev_id;
217 	u32 status, mask, pending;
218 	irqreturn_t ret = IRQ_NONE;
219 
220 	spin_lock(&isi->irqlock);
221 
222 	status = isi_readl(isi, ISI_STATUS);
223 	mask = isi_readl(isi, ISI_INTMASK);
224 	pending = status & mask;
225 
226 	if (pending & ISI_CTRL_SRST) {
227 		complete(&isi->complete);
228 		isi_writel(isi, ISI_INTDIS, ISI_CTRL_SRST);
229 		ret = IRQ_HANDLED;
230 	} else if (pending & ISI_CTRL_DIS) {
231 		complete(&isi->complete);
232 		isi_writel(isi, ISI_INTDIS, ISI_CTRL_DIS);
233 		ret = IRQ_HANDLED;
234 	} else {
235 		if (likely(pending & ISI_SR_CXFR_DONE) ||
236 				likely(pending & ISI_SR_PXFR_DONE))
237 			ret = atmel_isi_handle_streaming(isi);
238 	}
239 
240 	spin_unlock(&isi->irqlock);
241 	return ret;
242 }
243 
244 #define	WAIT_ISI_RESET		1
245 #define	WAIT_ISI_DISABLE	0
246 static int atmel_isi_wait_status(struct atmel_isi *isi, int wait_reset)
247 {
248 	unsigned long timeout;
249 	/*
250 	 * The reset or disable will only succeed if we have a
251 	 * pixel clock from the camera.
252 	 */
253 	init_completion(&isi->complete);
254 
255 	if (wait_reset) {
256 		isi_writel(isi, ISI_INTEN, ISI_CTRL_SRST);
257 		isi_writel(isi, ISI_CTRL, ISI_CTRL_SRST);
258 	} else {
259 		isi_writel(isi, ISI_INTEN, ISI_CTRL_DIS);
260 		isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
261 	}
262 
263 	timeout = wait_for_completion_timeout(&isi->complete,
264 			msecs_to_jiffies(500));
265 	if (timeout == 0)
266 		return -ETIMEDOUT;
267 
268 	return 0;
269 }
270 
271 /* ------------------------------------------------------------------
272 	Videobuf operations
273    ------------------------------------------------------------------*/
274 static int queue_setup(struct vb2_queue *vq,
275 				unsigned int *nbuffers, unsigned int *nplanes,
276 				unsigned int sizes[], struct device *alloc_devs[])
277 {
278 	struct atmel_isi *isi = vb2_get_drv_priv(vq);
279 	unsigned long size;
280 
281 	size = isi->fmt.fmt.pix.sizeimage;
282 
283 	/* Make sure the image size is large enough. */
284 	if (*nplanes)
285 		return sizes[0] < size ? -EINVAL : 0;
286 
287 	*nplanes = 1;
288 	sizes[0] = size;
289 
290 	isi->active = NULL;
291 
292 	dev_dbg(isi->dev, "%s, count=%d, size=%ld\n", __func__,
293 		*nbuffers, size);
294 
295 	return 0;
296 }
297 
298 static int buffer_init(struct vb2_buffer *vb)
299 {
300 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
301 	struct frame_buffer *buf = container_of(vbuf, struct frame_buffer, vb);
302 
303 	buf->p_dma_desc = NULL;
304 	INIT_LIST_HEAD(&buf->list);
305 
306 	return 0;
307 }
308 
309 static int buffer_prepare(struct vb2_buffer *vb)
310 {
311 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
312 	struct frame_buffer *buf = container_of(vbuf, struct frame_buffer, vb);
313 	struct atmel_isi *isi = vb2_get_drv_priv(vb->vb2_queue);
314 	unsigned long size;
315 	struct isi_dma_desc *desc;
316 
317 	size = isi->fmt.fmt.pix.sizeimage;
318 
319 	if (vb2_plane_size(vb, 0) < size) {
320 		dev_err(isi->dev, "%s data will not fit into plane (%lu < %lu)\n",
321 				__func__, vb2_plane_size(vb, 0), size);
322 		return -EINVAL;
323 	}
324 
325 	vb2_set_plane_payload(vb, 0, size);
326 
327 	if (!buf->p_dma_desc) {
328 		if (list_empty(&isi->dma_desc_head)) {
329 			dev_err(isi->dev, "Not enough dma descriptors.\n");
330 			return -EINVAL;
331 		} else {
332 			/* Get an available descriptor */
333 			desc = list_entry(isi->dma_desc_head.next,
334 						struct isi_dma_desc, list);
335 			/* Delete the descriptor since now it is used */
336 			list_del_init(&desc->list);
337 
338 			/* Initialize the dma descriptor */
339 			desc->p_fbd->fb_address =
340 					vb2_dma_contig_plane_dma_addr(vb, 0);
341 			desc->p_fbd->next_fbd_address = 0;
342 			set_dma_ctrl(desc->p_fbd, ISI_DMA_CTRL_WB);
343 
344 			buf->p_dma_desc = desc;
345 		}
346 	}
347 	return 0;
348 }
349 
350 static void buffer_cleanup(struct vb2_buffer *vb)
351 {
352 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
353 	struct atmel_isi *isi = vb2_get_drv_priv(vb->vb2_queue);
354 	struct frame_buffer *buf = container_of(vbuf, struct frame_buffer, vb);
355 
356 	/* This descriptor is available now and we add to head list */
357 	if (buf->p_dma_desc)
358 		list_add(&buf->p_dma_desc->list, &isi->dma_desc_head);
359 }
360 
361 static void start_dma(struct atmel_isi *isi, struct frame_buffer *buffer)
362 {
363 	u32 ctrl, cfg1;
364 
365 	cfg1 = isi_readl(isi, ISI_CFG1);
366 	/* Enable irq: cxfr for the codec path, pxfr for the preview path */
367 	isi_writel(isi, ISI_INTEN,
368 			ISI_SR_CXFR_DONE | ISI_SR_PXFR_DONE);
369 
370 	/* Check if already in a frame */
371 	if (!isi->enable_preview_path) {
372 		if (isi_readl(isi, ISI_STATUS) & ISI_CTRL_CDC) {
373 			dev_err(isi->dev, "Already in frame handling.\n");
374 			return;
375 		}
376 
377 		isi_writel(isi, ISI_DMA_C_DSCR,
378 				(u32)buffer->p_dma_desc->fbd_phys);
379 		isi_writel(isi, ISI_DMA_C_CTRL,
380 				ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
381 		isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_C_CH);
382 	} else {
383 		isi_writel(isi, ISI_DMA_P_DSCR,
384 				(u32)buffer->p_dma_desc->fbd_phys);
385 		isi_writel(isi, ISI_DMA_P_CTRL,
386 				ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
387 		isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_P_CH);
388 	}
389 
390 	cfg1 &= ~ISI_CFG1_FRATE_DIV_MASK;
391 	/* Enable linked list */
392 	cfg1 |= isi->pdata.frate | ISI_CFG1_DISCR;
393 
394 	/* Enable ISI */
395 	ctrl = ISI_CTRL_EN;
396 
397 	if (!isi->enable_preview_path)
398 		ctrl |= ISI_CTRL_CDC;
399 
400 	isi_writel(isi, ISI_CTRL, ctrl);
401 	isi_writel(isi, ISI_CFG1, cfg1);
402 }
403 
404 static void buffer_queue(struct vb2_buffer *vb)
405 {
406 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
407 	struct atmel_isi *isi = vb2_get_drv_priv(vb->vb2_queue);
408 	struct frame_buffer *buf = container_of(vbuf, struct frame_buffer, vb);
409 	unsigned long flags = 0;
410 
411 	spin_lock_irqsave(&isi->irqlock, flags);
412 	list_add_tail(&buf->list, &isi->video_buffer_list);
413 
414 	if (isi->active == NULL) {
415 		isi->active = buf;
416 		if (vb2_is_streaming(vb->vb2_queue))
417 			start_dma(isi, buf);
418 	}
419 	spin_unlock_irqrestore(&isi->irqlock, flags);
420 }
421 
422 static int start_streaming(struct vb2_queue *vq, unsigned int count)
423 {
424 	struct atmel_isi *isi = vb2_get_drv_priv(vq);
425 	struct frame_buffer *buf, *node;
426 	int ret;
427 
428 	pm_runtime_get_sync(isi->dev);
429 
430 	/* Enable stream on the sub device */
431 	ret = v4l2_subdev_call(isi->entity.subdev, video, s_stream, 1);
432 	if (ret && ret != -ENOIOCTLCMD) {
433 		dev_err(isi->dev, "stream on failed in subdev\n");
434 		goto err_start_stream;
435 	}
436 
437 	/* Reset ISI */
438 	ret = atmel_isi_wait_status(isi, WAIT_ISI_RESET);
439 	if (ret < 0) {
440 		dev_err(isi->dev, "Reset ISI timed out\n");
441 		goto err_reset;
442 	}
443 	/* Disable all interrupts */
444 	isi_writel(isi, ISI_INTDIS, (u32)~0UL);
445 
446 	isi->sequence = 0;
447 	configure_geometry(isi);
448 
449 	spin_lock_irq(&isi->irqlock);
450 	/* Clear any pending interrupt */
451 	isi_readl(isi, ISI_STATUS);
452 
453 	start_dma(isi, isi->active);
454 	spin_unlock_irq(&isi->irqlock);
455 
456 	return 0;
457 
458 err_reset:
459 	v4l2_subdev_call(isi->entity.subdev, video, s_stream, 0);
460 
461 err_start_stream:
462 	pm_runtime_put(isi->dev);
463 
464 	spin_lock_irq(&isi->irqlock);
465 	isi->active = NULL;
466 	/* Release all active buffers */
467 	list_for_each_entry_safe(buf, node, &isi->video_buffer_list, list) {
468 		list_del_init(&buf->list);
469 		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
470 	}
471 	spin_unlock_irq(&isi->irqlock);
472 
473 	return ret;
474 }
475 
476 /* abort streaming and wait for last buffer */
477 static void stop_streaming(struct vb2_queue *vq)
478 {
479 	struct atmel_isi *isi = vb2_get_drv_priv(vq);
480 	struct frame_buffer *buf, *node;
481 	int ret = 0;
482 	unsigned long timeout;
483 
484 	/* Disable stream on the sub device */
485 	ret = v4l2_subdev_call(isi->entity.subdev, video, s_stream, 0);
486 	if (ret && ret != -ENOIOCTLCMD)
487 		dev_err(isi->dev, "stream off failed in subdev\n");
488 
489 	spin_lock_irq(&isi->irqlock);
490 	isi->active = NULL;
491 	/* Release all active buffers */
492 	list_for_each_entry_safe(buf, node, &isi->video_buffer_list, list) {
493 		list_del_init(&buf->list);
494 		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
495 	}
496 	spin_unlock_irq(&isi->irqlock);
497 
498 	if (!isi->enable_preview_path) {
499 		timeout = jiffies + FRAME_INTERVAL_MILLI_SEC * HZ;
500 		/* Wait until the end of the current frame. */
501 		while ((isi_readl(isi, ISI_STATUS) & ISI_CTRL_CDC) &&
502 				time_before(jiffies, timeout))
503 			msleep(1);
504 
505 		if (time_after(jiffies, timeout))
506 			dev_err(isi->dev,
507 				"Timeout waiting for finishing codec request\n");
508 	}
509 
510 	/* Disable interrupts */
511 	isi_writel(isi, ISI_INTDIS,
512 			ISI_SR_CXFR_DONE | ISI_SR_PXFR_DONE);
513 
514 	/* Disable ISI and wait for it is done */
515 	ret = atmel_isi_wait_status(isi, WAIT_ISI_DISABLE);
516 	if (ret < 0)
517 		dev_err(isi->dev, "Disable ISI timed out\n");
518 
519 	pm_runtime_put(isi->dev);
520 }
521 
522 static const struct vb2_ops isi_video_qops = {
523 	.queue_setup		= queue_setup,
524 	.buf_init		= buffer_init,
525 	.buf_prepare		= buffer_prepare,
526 	.buf_cleanup		= buffer_cleanup,
527 	.buf_queue		= buffer_queue,
528 	.start_streaming	= start_streaming,
529 	.stop_streaming		= stop_streaming,
530 	.wait_prepare		= vb2_ops_wait_prepare,
531 	.wait_finish		= vb2_ops_wait_finish,
532 };
533 
534 static int isi_g_fmt_vid_cap(struct file *file, void *priv,
535 			      struct v4l2_format *fmt)
536 {
537 	struct atmel_isi *isi = video_drvdata(file);
538 
539 	*fmt = isi->fmt;
540 
541 	return 0;
542 }
543 
544 static const struct isi_format *find_format_by_fourcc(struct atmel_isi *isi,
545 						      unsigned int fourcc)
546 {
547 	unsigned int num_formats = isi->num_user_formats;
548 	const struct isi_format *fmt;
549 	unsigned int i;
550 
551 	for (i = 0; i < num_formats; i++) {
552 		fmt = isi->user_formats[i];
553 		if (fmt->fourcc == fourcc)
554 			return fmt;
555 	}
556 
557 	return NULL;
558 }
559 
560 static int isi_try_fmt(struct atmel_isi *isi, struct v4l2_format *f,
561 		       const struct isi_format **current_fmt)
562 {
563 	const struct isi_format *isi_fmt;
564 	struct v4l2_pix_format *pixfmt = &f->fmt.pix;
565 	struct v4l2_subdev_pad_config pad_cfg;
566 	struct v4l2_subdev_format format = {
567 		.which = V4L2_SUBDEV_FORMAT_TRY,
568 	};
569 	int ret;
570 
571 	isi_fmt = find_format_by_fourcc(isi, pixfmt->pixelformat);
572 	if (!isi_fmt) {
573 		isi_fmt = isi->user_formats[isi->num_user_formats - 1];
574 		pixfmt->pixelformat = isi_fmt->fourcc;
575 	}
576 
577 	/* Limit to Atmel ISI hardware capabilities */
578 	pixfmt->width = clamp(pixfmt->width, 0U, MAX_SUPPORT_WIDTH);
579 	pixfmt->height = clamp(pixfmt->height, 0U, MAX_SUPPORT_HEIGHT);
580 
581 	v4l2_fill_mbus_format(&format.format, pixfmt, isi_fmt->mbus_code);
582 	ret = v4l2_subdev_call(isi->entity.subdev, pad, set_fmt,
583 			       &pad_cfg, &format);
584 	if (ret < 0)
585 		return ret;
586 
587 	v4l2_fill_pix_format(pixfmt, &format.format);
588 
589 	pixfmt->field = V4L2_FIELD_NONE;
590 	pixfmt->bytesperline = pixfmt->width * isi_fmt->bpp;
591 	pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
592 
593 	if (current_fmt)
594 		*current_fmt = isi_fmt;
595 
596 	return 0;
597 }
598 
599 static int isi_set_fmt(struct atmel_isi *isi, struct v4l2_format *f)
600 {
601 	struct v4l2_subdev_format format = {
602 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
603 	};
604 	const struct isi_format *current_fmt;
605 	int ret;
606 
607 	ret = isi_try_fmt(isi, f, &current_fmt);
608 	if (ret)
609 		return ret;
610 
611 	v4l2_fill_mbus_format(&format.format, &f->fmt.pix,
612 			      current_fmt->mbus_code);
613 	ret = v4l2_subdev_call(isi->entity.subdev, pad,
614 			       set_fmt, NULL, &format);
615 	if (ret < 0)
616 		return ret;
617 
618 	isi->fmt = *f;
619 	isi->current_fmt = current_fmt;
620 
621 	return 0;
622 }
623 
624 static int isi_s_fmt_vid_cap(struct file *file, void *priv,
625 			      struct v4l2_format *f)
626 {
627 	struct atmel_isi *isi = video_drvdata(file);
628 
629 	if (vb2_is_streaming(&isi->queue))
630 		return -EBUSY;
631 
632 	return isi_set_fmt(isi, f);
633 }
634 
635 static int isi_try_fmt_vid_cap(struct file *file, void *priv,
636 				struct v4l2_format *f)
637 {
638 	struct atmel_isi *isi = video_drvdata(file);
639 
640 	return isi_try_fmt(isi, f, NULL);
641 }
642 
643 static int isi_enum_fmt_vid_cap(struct file *file, void  *priv,
644 				struct v4l2_fmtdesc *f)
645 {
646 	struct atmel_isi *isi = video_drvdata(file);
647 
648 	if (f->index >= isi->num_user_formats)
649 		return -EINVAL;
650 
651 	f->pixelformat = isi->user_formats[f->index]->fourcc;
652 	return 0;
653 }
654 
655 static int isi_querycap(struct file *file, void *priv,
656 			struct v4l2_capability *cap)
657 {
658 	strlcpy(cap->driver, "atmel-isi", sizeof(cap->driver));
659 	strlcpy(cap->card, "Atmel Image Sensor Interface", sizeof(cap->card));
660 	strlcpy(cap->bus_info, "platform:isi", sizeof(cap->bus_info));
661 	return 0;
662 }
663 
664 static int isi_enum_input(struct file *file, void *priv,
665 			   struct v4l2_input *i)
666 {
667 	if (i->index != 0)
668 		return -EINVAL;
669 
670 	i->type = V4L2_INPUT_TYPE_CAMERA;
671 	strlcpy(i->name, "Camera", sizeof(i->name));
672 	return 0;
673 }
674 
675 static int isi_g_input(struct file *file, void *priv, unsigned int *i)
676 {
677 	*i = 0;
678 	return 0;
679 }
680 
681 static int isi_s_input(struct file *file, void *priv, unsigned int i)
682 {
683 	if (i > 0)
684 		return -EINVAL;
685 	return 0;
686 }
687 
688 static int isi_g_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
689 {
690 	struct atmel_isi *isi = video_drvdata(file);
691 
692 	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
693 		return -EINVAL;
694 
695 	a->parm.capture.readbuffers = 2;
696 	return v4l2_subdev_call(isi->entity.subdev, video, g_parm, a);
697 }
698 
699 static int isi_s_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
700 {
701 	struct atmel_isi *isi = video_drvdata(file);
702 
703 	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
704 		return -EINVAL;
705 
706 	a->parm.capture.readbuffers = 2;
707 	return v4l2_subdev_call(isi->entity.subdev, video, s_parm, a);
708 }
709 
710 static int isi_enum_framesizes(struct file *file, void *fh,
711 			       struct v4l2_frmsizeenum *fsize)
712 {
713 	struct atmel_isi *isi = video_drvdata(file);
714 	const struct isi_format *isi_fmt;
715 	struct v4l2_subdev_frame_size_enum fse = {
716 		.index = fsize->index,
717 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
718 	};
719 	int ret;
720 
721 	isi_fmt = find_format_by_fourcc(isi, fsize->pixel_format);
722 	if (!isi_fmt)
723 		return -EINVAL;
724 
725 	fse.code = isi_fmt->mbus_code;
726 
727 	ret = v4l2_subdev_call(isi->entity.subdev, pad, enum_frame_size,
728 			       NULL, &fse);
729 	if (ret)
730 		return ret;
731 
732 	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
733 	fsize->discrete.width = fse.max_width;
734 	fsize->discrete.height = fse.max_height;
735 
736 	return 0;
737 }
738 
739 static int isi_enum_frameintervals(struct file *file, void *fh,
740 				    struct v4l2_frmivalenum *fival)
741 {
742 	struct atmel_isi *isi = video_drvdata(file);
743 	const struct isi_format *isi_fmt;
744 	struct v4l2_subdev_frame_interval_enum fie = {
745 		.index = fival->index,
746 		.width = fival->width,
747 		.height = fival->height,
748 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
749 	};
750 	int ret;
751 
752 	isi_fmt = find_format_by_fourcc(isi, fival->pixel_format);
753 	if (!isi_fmt)
754 		return -EINVAL;
755 
756 	fie.code = isi_fmt->mbus_code;
757 
758 	ret = v4l2_subdev_call(isi->entity.subdev, pad,
759 			       enum_frame_interval, NULL, &fie);
760 	if (ret)
761 		return ret;
762 
763 	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
764 	fival->discrete = fie.interval;
765 
766 	return 0;
767 }
768 
769 static void isi_camera_set_bus_param(struct atmel_isi *isi)
770 {
771 	u32 cfg1 = 0;
772 
773 	/* set bus param for ISI */
774 	if (isi->pdata.hsync_act_low)
775 		cfg1 |= ISI_CFG1_HSYNC_POL_ACTIVE_LOW;
776 	if (isi->pdata.vsync_act_low)
777 		cfg1 |= ISI_CFG1_VSYNC_POL_ACTIVE_LOW;
778 	if (isi->pdata.pclk_act_falling)
779 		cfg1 |= ISI_CFG1_PIXCLK_POL_ACTIVE_FALLING;
780 	if (isi->pdata.has_emb_sync)
781 		cfg1 |= ISI_CFG1_EMB_SYNC;
782 	if (isi->pdata.full_mode)
783 		cfg1 |= ISI_CFG1_FULL_MODE;
784 
785 	cfg1 |= ISI_CFG1_THMASK_BEATS_16;
786 
787 	/* Enable PM and peripheral clock before operate isi registers */
788 	pm_runtime_get_sync(isi->dev);
789 
790 	isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
791 	isi_writel(isi, ISI_CFG1, cfg1);
792 
793 	pm_runtime_put(isi->dev);
794 }
795 
796 /* -----------------------------------------------------------------------*/
797 static int atmel_isi_parse_dt(struct atmel_isi *isi,
798 			struct platform_device *pdev)
799 {
800 	struct device_node *np = pdev->dev.of_node;
801 	struct v4l2_fwnode_endpoint ep;
802 	int err;
803 
804 	/* Default settings for ISI */
805 	isi->pdata.full_mode = 1;
806 	isi->pdata.frate = ISI_CFG1_FRATE_CAPTURE_ALL;
807 
808 	np = of_graph_get_next_endpoint(np, NULL);
809 	if (!np) {
810 		dev_err(&pdev->dev, "Could not find the endpoint\n");
811 		return -EINVAL;
812 	}
813 
814 	err = v4l2_fwnode_endpoint_parse(of_fwnode_handle(np), &ep);
815 	of_node_put(np);
816 	if (err) {
817 		dev_err(&pdev->dev, "Could not parse the endpoint\n");
818 		return err;
819 	}
820 
821 	switch (ep.bus.parallel.bus_width) {
822 	case 8:
823 		isi->pdata.data_width_flags = ISI_DATAWIDTH_8;
824 		break;
825 	case 10:
826 		isi->pdata.data_width_flags =
827 				ISI_DATAWIDTH_8 | ISI_DATAWIDTH_10;
828 		break;
829 	default:
830 		dev_err(&pdev->dev, "Unsupported bus width: %d\n",
831 				ep.bus.parallel.bus_width);
832 		return -EINVAL;
833 	}
834 
835 	if (ep.bus.parallel.flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
836 		isi->pdata.hsync_act_low = true;
837 	if (ep.bus.parallel.flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
838 		isi->pdata.vsync_act_low = true;
839 	if (ep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
840 		isi->pdata.pclk_act_falling = true;
841 
842 	if (ep.bus_type == V4L2_MBUS_BT656)
843 		isi->pdata.has_emb_sync = true;
844 
845 	return 0;
846 }
847 
848 static int isi_open(struct file *file)
849 {
850 	struct atmel_isi *isi = video_drvdata(file);
851 	struct v4l2_subdev *sd = isi->entity.subdev;
852 	int ret;
853 
854 	if (mutex_lock_interruptible(&isi->lock))
855 		return -ERESTARTSYS;
856 
857 	ret = v4l2_fh_open(file);
858 	if (ret < 0)
859 		goto unlock;
860 
861 	if (!v4l2_fh_is_singular_file(file))
862 		goto fh_rel;
863 
864 	ret = v4l2_subdev_call(sd, core, s_power, 1);
865 	if (ret < 0 && ret != -ENOIOCTLCMD)
866 		goto fh_rel;
867 
868 	ret = isi_set_fmt(isi, &isi->fmt);
869 	if (ret)
870 		v4l2_subdev_call(sd, core, s_power, 0);
871 fh_rel:
872 	if (ret)
873 		v4l2_fh_release(file);
874 unlock:
875 	mutex_unlock(&isi->lock);
876 	return ret;
877 }
878 
879 static int isi_release(struct file *file)
880 {
881 	struct atmel_isi *isi = video_drvdata(file);
882 	struct v4l2_subdev *sd = isi->entity.subdev;
883 	bool fh_singular;
884 	int ret;
885 
886 	mutex_lock(&isi->lock);
887 
888 	fh_singular = v4l2_fh_is_singular_file(file);
889 
890 	ret = _vb2_fop_release(file, NULL);
891 
892 	if (fh_singular)
893 		v4l2_subdev_call(sd, core, s_power, 0);
894 
895 	mutex_unlock(&isi->lock);
896 
897 	return ret;
898 }
899 
900 static const struct v4l2_ioctl_ops isi_ioctl_ops = {
901 	.vidioc_querycap		= isi_querycap,
902 
903 	.vidioc_try_fmt_vid_cap		= isi_try_fmt_vid_cap,
904 	.vidioc_g_fmt_vid_cap		= isi_g_fmt_vid_cap,
905 	.vidioc_s_fmt_vid_cap		= isi_s_fmt_vid_cap,
906 	.vidioc_enum_fmt_vid_cap	= isi_enum_fmt_vid_cap,
907 
908 	.vidioc_enum_input		= isi_enum_input,
909 	.vidioc_g_input			= isi_g_input,
910 	.vidioc_s_input			= isi_s_input,
911 
912 	.vidioc_g_parm			= isi_g_parm,
913 	.vidioc_s_parm			= isi_s_parm,
914 	.vidioc_enum_framesizes		= isi_enum_framesizes,
915 	.vidioc_enum_frameintervals	= isi_enum_frameintervals,
916 
917 	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
918 	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
919 	.vidioc_querybuf		= vb2_ioctl_querybuf,
920 	.vidioc_qbuf			= vb2_ioctl_qbuf,
921 	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
922 	.vidioc_expbuf			= vb2_ioctl_expbuf,
923 	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
924 	.vidioc_streamon		= vb2_ioctl_streamon,
925 	.vidioc_streamoff		= vb2_ioctl_streamoff,
926 
927 	.vidioc_log_status		= v4l2_ctrl_log_status,
928 	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
929 	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
930 };
931 
932 static const struct v4l2_file_operations isi_fops = {
933 	.owner		= THIS_MODULE,
934 	.unlocked_ioctl	= video_ioctl2,
935 	.open		= isi_open,
936 	.release	= isi_release,
937 	.poll		= vb2_fop_poll,
938 	.mmap		= vb2_fop_mmap,
939 	.read		= vb2_fop_read,
940 };
941 
942 static int isi_set_default_fmt(struct atmel_isi *isi)
943 {
944 	struct v4l2_format f = {
945 		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
946 		.fmt.pix = {
947 			.width		= VGA_WIDTH,
948 			.height		= VGA_HEIGHT,
949 			.field		= V4L2_FIELD_NONE,
950 			.pixelformat	= isi->user_formats[0]->fourcc,
951 		},
952 	};
953 	int ret;
954 
955 	ret = isi_try_fmt(isi, &f, NULL);
956 	if (ret)
957 		return ret;
958 	isi->current_fmt = isi->user_formats[0];
959 	isi->fmt = f;
960 	return 0;
961 }
962 
963 static const struct isi_format isi_formats[] = {
964 	{
965 		.fourcc = V4L2_PIX_FMT_YUYV,
966 		.mbus_code = MEDIA_BUS_FMT_YUYV8_2X8,
967 		.bpp = 2,
968 		.swap = ISI_CFG2_YCC_SWAP_DEFAULT,
969 	}, {
970 		.fourcc = V4L2_PIX_FMT_YUYV,
971 		.mbus_code = MEDIA_BUS_FMT_YVYU8_2X8,
972 		.bpp = 2,
973 		.swap = ISI_CFG2_YCC_SWAP_MODE_1,
974 	}, {
975 		.fourcc = V4L2_PIX_FMT_YUYV,
976 		.mbus_code = MEDIA_BUS_FMT_UYVY8_2X8,
977 		.bpp = 2,
978 		.swap = ISI_CFG2_YCC_SWAP_MODE_2,
979 	}, {
980 		.fourcc = V4L2_PIX_FMT_YUYV,
981 		.mbus_code = MEDIA_BUS_FMT_VYUY8_2X8,
982 		.bpp = 2,
983 		.swap = ISI_CFG2_YCC_SWAP_MODE_3,
984 	}, {
985 		.fourcc = V4L2_PIX_FMT_RGB565,
986 		.mbus_code = MEDIA_BUS_FMT_YUYV8_2X8,
987 		.bpp = 2,
988 		.swap = ISI_CFG2_YCC_SWAP_MODE_2,
989 	}, {
990 		.fourcc = V4L2_PIX_FMT_RGB565,
991 		.mbus_code = MEDIA_BUS_FMT_YVYU8_2X8,
992 		.bpp = 2,
993 		.swap = ISI_CFG2_YCC_SWAP_MODE_3,
994 	}, {
995 		.fourcc = V4L2_PIX_FMT_RGB565,
996 		.mbus_code = MEDIA_BUS_FMT_UYVY8_2X8,
997 		.bpp = 2,
998 		.swap = ISI_CFG2_YCC_SWAP_DEFAULT,
999 	}, {
1000 		.fourcc = V4L2_PIX_FMT_RGB565,
1001 		.mbus_code = MEDIA_BUS_FMT_VYUY8_2X8,
1002 		.bpp = 2,
1003 		.swap = ISI_CFG2_YCC_SWAP_MODE_1,
1004 	},
1005 };
1006 
1007 static int isi_formats_init(struct atmel_isi *isi)
1008 {
1009 	const struct isi_format *isi_fmts[ARRAY_SIZE(isi_formats)];
1010 	unsigned int num_fmts = 0, i, j;
1011 	struct v4l2_subdev *subdev = isi->entity.subdev;
1012 	struct v4l2_subdev_mbus_code_enum mbus_code = {
1013 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
1014 	};
1015 
1016 	while (!v4l2_subdev_call(subdev, pad, enum_mbus_code,
1017 				 NULL, &mbus_code)) {
1018 		for (i = 0; i < ARRAY_SIZE(isi_formats); i++) {
1019 			if (isi_formats[i].mbus_code != mbus_code.code)
1020 				continue;
1021 
1022 			/* Code supported, have we got this fourcc yet? */
1023 			for (j = 0; j < num_fmts; j++)
1024 				if (isi_fmts[j]->fourcc == isi_formats[i].fourcc)
1025 					/* Already available */
1026 					break;
1027 			if (j == num_fmts)
1028 				/* new */
1029 				isi_fmts[num_fmts++] = isi_formats + i;
1030 		}
1031 		mbus_code.index++;
1032 	}
1033 
1034 	if (!num_fmts)
1035 		return -ENXIO;
1036 
1037 	isi->num_user_formats = num_fmts;
1038 	isi->user_formats = devm_kcalloc(isi->dev,
1039 					 num_fmts, sizeof(struct isi_format *),
1040 					 GFP_KERNEL);
1041 	if (!isi->user_formats) {
1042 		dev_err(isi->dev, "could not allocate memory\n");
1043 		return -ENOMEM;
1044 	}
1045 
1046 	memcpy(isi->user_formats, isi_fmts,
1047 	       num_fmts * sizeof(struct isi_format *));
1048 	isi->current_fmt = isi->user_formats[0];
1049 
1050 	return 0;
1051 }
1052 
1053 static int isi_graph_notify_complete(struct v4l2_async_notifier *notifier)
1054 {
1055 	struct atmel_isi *isi = notifier_to_isi(notifier);
1056 	int ret;
1057 
1058 	isi->vdev->ctrl_handler = isi->entity.subdev->ctrl_handler;
1059 	ret = isi_formats_init(isi);
1060 	if (ret) {
1061 		dev_err(isi->dev, "No supported mediabus format found\n");
1062 		return ret;
1063 	}
1064 	isi_camera_set_bus_param(isi);
1065 
1066 	ret = isi_set_default_fmt(isi);
1067 	if (ret) {
1068 		dev_err(isi->dev, "Could not set default format\n");
1069 		return ret;
1070 	}
1071 
1072 	ret = video_register_device(isi->vdev, VFL_TYPE_GRABBER, -1);
1073 	if (ret) {
1074 		dev_err(isi->dev, "Failed to register video device\n");
1075 		return ret;
1076 	}
1077 
1078 	dev_dbg(isi->dev, "Device registered as %s\n",
1079 		video_device_node_name(isi->vdev));
1080 	return 0;
1081 }
1082 
1083 static void isi_graph_notify_unbind(struct v4l2_async_notifier *notifier,
1084 				     struct v4l2_subdev *sd,
1085 				     struct v4l2_async_subdev *asd)
1086 {
1087 	struct atmel_isi *isi = notifier_to_isi(notifier);
1088 
1089 	dev_dbg(isi->dev, "Removing %s\n", video_device_node_name(isi->vdev));
1090 
1091 	/* Checks internaly if vdev have been init or not */
1092 	video_unregister_device(isi->vdev);
1093 }
1094 
1095 static int isi_graph_notify_bound(struct v4l2_async_notifier *notifier,
1096 				   struct v4l2_subdev *subdev,
1097 				   struct v4l2_async_subdev *asd)
1098 {
1099 	struct atmel_isi *isi = notifier_to_isi(notifier);
1100 
1101 	dev_dbg(isi->dev, "subdev %s bound\n", subdev->name);
1102 
1103 	isi->entity.subdev = subdev;
1104 
1105 	return 0;
1106 }
1107 
1108 static int isi_graph_parse(struct atmel_isi *isi, struct device_node *node)
1109 {
1110 	struct device_node *ep = NULL;
1111 	struct device_node *remote;
1112 
1113 	while (1) {
1114 		ep = of_graph_get_next_endpoint(node, ep);
1115 		if (!ep)
1116 			return -EINVAL;
1117 
1118 		remote = of_graph_get_remote_port_parent(ep);
1119 		if (!remote) {
1120 			of_node_put(ep);
1121 			return -EINVAL;
1122 		}
1123 
1124 		/* Remote node to connect */
1125 		isi->entity.node = remote;
1126 		isi->entity.asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
1127 		isi->entity.asd.match.fwnode.fwnode = of_fwnode_handle(remote);
1128 		return 0;
1129 	}
1130 }
1131 
1132 static int isi_graph_init(struct atmel_isi *isi)
1133 {
1134 	struct v4l2_async_subdev **subdevs = NULL;
1135 	int ret;
1136 
1137 	/* Parse the graph to extract a list of subdevice DT nodes. */
1138 	ret = isi_graph_parse(isi, isi->dev->of_node);
1139 	if (ret < 0) {
1140 		dev_err(isi->dev, "Graph parsing failed\n");
1141 		return ret;
1142 	}
1143 
1144 	/* Register the subdevices notifier. */
1145 	subdevs = devm_kzalloc(isi->dev, sizeof(*subdevs), GFP_KERNEL);
1146 	if (subdevs == NULL) {
1147 		of_node_put(isi->entity.node);
1148 		return -ENOMEM;
1149 	}
1150 
1151 	subdevs[0] = &isi->entity.asd;
1152 
1153 	isi->notifier.subdevs = subdevs;
1154 	isi->notifier.num_subdevs = 1;
1155 	isi->notifier.bound = isi_graph_notify_bound;
1156 	isi->notifier.unbind = isi_graph_notify_unbind;
1157 	isi->notifier.complete = isi_graph_notify_complete;
1158 
1159 	ret = v4l2_async_notifier_register(&isi->v4l2_dev, &isi->notifier);
1160 	if (ret < 0) {
1161 		dev_err(isi->dev, "Notifier registration failed\n");
1162 		of_node_put(isi->entity.node);
1163 		return ret;
1164 	}
1165 
1166 	return 0;
1167 }
1168 
1169 
1170 static int atmel_isi_probe(struct platform_device *pdev)
1171 {
1172 	int irq;
1173 	struct atmel_isi *isi;
1174 	struct vb2_queue *q;
1175 	struct resource *regs;
1176 	int ret, i;
1177 
1178 	isi = devm_kzalloc(&pdev->dev, sizeof(struct atmel_isi), GFP_KERNEL);
1179 	if (!isi) {
1180 		dev_err(&pdev->dev, "Can't allocate interface!\n");
1181 		return -ENOMEM;
1182 	}
1183 
1184 	isi->pclk = devm_clk_get(&pdev->dev, "isi_clk");
1185 	if (IS_ERR(isi->pclk))
1186 		return PTR_ERR(isi->pclk);
1187 
1188 	ret = atmel_isi_parse_dt(isi, pdev);
1189 	if (ret)
1190 		return ret;
1191 
1192 	isi->active = NULL;
1193 	isi->dev = &pdev->dev;
1194 	mutex_init(&isi->lock);
1195 	spin_lock_init(&isi->irqlock);
1196 	INIT_LIST_HEAD(&isi->video_buffer_list);
1197 	INIT_LIST_HEAD(&isi->dma_desc_head);
1198 
1199 	q = &isi->queue;
1200 
1201 	/* Initialize the top-level structure */
1202 	ret = v4l2_device_register(&pdev->dev, &isi->v4l2_dev);
1203 	if (ret)
1204 		return ret;
1205 
1206 	isi->vdev = video_device_alloc();
1207 	if (isi->vdev == NULL) {
1208 		ret = -ENOMEM;
1209 		goto err_vdev_alloc;
1210 	}
1211 
1212 	/* video node */
1213 	isi->vdev->fops = &isi_fops;
1214 	isi->vdev->v4l2_dev = &isi->v4l2_dev;
1215 	isi->vdev->queue = &isi->queue;
1216 	strlcpy(isi->vdev->name, KBUILD_MODNAME, sizeof(isi->vdev->name));
1217 	isi->vdev->release = video_device_release;
1218 	isi->vdev->ioctl_ops = &isi_ioctl_ops;
1219 	isi->vdev->lock = &isi->lock;
1220 	isi->vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
1221 		V4L2_CAP_READWRITE;
1222 	video_set_drvdata(isi->vdev, isi);
1223 
1224 	/* buffer queue */
1225 	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1226 	q->io_modes = VB2_MMAP | VB2_READ | VB2_DMABUF;
1227 	q->lock = &isi->lock;
1228 	q->drv_priv = isi;
1229 	q->buf_struct_size = sizeof(struct frame_buffer);
1230 	q->ops = &isi_video_qops;
1231 	q->mem_ops = &vb2_dma_contig_memops;
1232 	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1233 	q->min_buffers_needed = 2;
1234 	q->dev = &pdev->dev;
1235 
1236 	ret = vb2_queue_init(q);
1237 	if (ret < 0) {
1238 		dev_err(&pdev->dev, "failed to initialize VB2 queue\n");
1239 		goto err_vb2_queue;
1240 	}
1241 	isi->p_fb_descriptors = dma_alloc_coherent(&pdev->dev,
1242 				sizeof(struct fbd) * VIDEO_MAX_FRAME,
1243 				&isi->fb_descriptors_phys,
1244 				GFP_KERNEL);
1245 	if (!isi->p_fb_descriptors) {
1246 		dev_err(&pdev->dev, "Can't allocate descriptors!\n");
1247 		ret = -ENOMEM;
1248 		goto err_dma_alloc;
1249 	}
1250 
1251 	for (i = 0; i < VIDEO_MAX_FRAME; i++) {
1252 		isi->dma_desc[i].p_fbd = isi->p_fb_descriptors + i;
1253 		isi->dma_desc[i].fbd_phys = isi->fb_descriptors_phys +
1254 					i * sizeof(struct fbd);
1255 		list_add(&isi->dma_desc[i].list, &isi->dma_desc_head);
1256 	}
1257 
1258 	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1259 	isi->regs = devm_ioremap_resource(&pdev->dev, regs);
1260 	if (IS_ERR(isi->regs)) {
1261 		ret = PTR_ERR(isi->regs);
1262 		goto err_ioremap;
1263 	}
1264 
1265 	if (isi->pdata.data_width_flags & ISI_DATAWIDTH_8)
1266 		isi->width_flags = 1 << 7;
1267 	if (isi->pdata.data_width_flags & ISI_DATAWIDTH_10)
1268 		isi->width_flags |= 1 << 9;
1269 
1270 	irq = platform_get_irq(pdev, 0);
1271 	if (irq < 0) {
1272 		ret = irq;
1273 		goto err_req_irq;
1274 	}
1275 
1276 	ret = devm_request_irq(&pdev->dev, irq, isi_interrupt, 0, "isi", isi);
1277 	if (ret) {
1278 		dev_err(&pdev->dev, "Unable to request irq %d\n", irq);
1279 		goto err_req_irq;
1280 	}
1281 	isi->irq = irq;
1282 
1283 	ret = isi_graph_init(isi);
1284 	if (ret < 0)
1285 		goto err_req_irq;
1286 
1287 	pm_suspend_ignore_children(&pdev->dev, true);
1288 	pm_runtime_enable(&pdev->dev);
1289 	platform_set_drvdata(pdev, isi);
1290 	return 0;
1291 
1292 err_req_irq:
1293 err_ioremap:
1294 	dma_free_coherent(&pdev->dev,
1295 			sizeof(struct fbd) * VIDEO_MAX_FRAME,
1296 			isi->p_fb_descriptors,
1297 			isi->fb_descriptors_phys);
1298 err_dma_alloc:
1299 err_vb2_queue:
1300 	video_device_release(isi->vdev);
1301 err_vdev_alloc:
1302 	v4l2_device_unregister(&isi->v4l2_dev);
1303 
1304 	return ret;
1305 }
1306 
1307 static int atmel_isi_remove(struct platform_device *pdev)
1308 {
1309 	struct atmel_isi *isi = platform_get_drvdata(pdev);
1310 
1311 	dma_free_coherent(&pdev->dev,
1312 			sizeof(struct fbd) * VIDEO_MAX_FRAME,
1313 			isi->p_fb_descriptors,
1314 			isi->fb_descriptors_phys);
1315 	pm_runtime_disable(&pdev->dev);
1316 	v4l2_async_notifier_unregister(&isi->notifier);
1317 	v4l2_device_unregister(&isi->v4l2_dev);
1318 
1319 	return 0;
1320 }
1321 
1322 #ifdef CONFIG_PM
1323 static int atmel_isi_runtime_suspend(struct device *dev)
1324 {
1325 	struct atmel_isi *isi = dev_get_drvdata(dev);
1326 
1327 	clk_disable_unprepare(isi->pclk);
1328 
1329 	return 0;
1330 }
1331 static int atmel_isi_runtime_resume(struct device *dev)
1332 {
1333 	struct atmel_isi *isi = dev_get_drvdata(dev);
1334 
1335 	return clk_prepare_enable(isi->pclk);
1336 }
1337 #endif /* CONFIG_PM */
1338 
1339 static const struct dev_pm_ops atmel_isi_dev_pm_ops = {
1340 	SET_RUNTIME_PM_OPS(atmel_isi_runtime_suspend,
1341 				atmel_isi_runtime_resume, NULL)
1342 };
1343 
1344 static const struct of_device_id atmel_isi_of_match[] = {
1345 	{ .compatible = "atmel,at91sam9g45-isi" },
1346 	{ }
1347 };
1348 MODULE_DEVICE_TABLE(of, atmel_isi_of_match);
1349 
1350 static struct platform_driver atmel_isi_driver = {
1351 	.driver		= {
1352 		.name = "atmel_isi",
1353 		.of_match_table = of_match_ptr(atmel_isi_of_match),
1354 		.pm	= &atmel_isi_dev_pm_ops,
1355 	},
1356 	.probe		= atmel_isi_probe,
1357 	.remove		= atmel_isi_remove,
1358 };
1359 
1360 module_platform_driver(atmel_isi_driver);
1361 
1362 MODULE_AUTHOR("Josh Wu <josh.wu@atmel.com>");
1363 MODULE_DESCRIPTION("The V4L2 driver for Atmel Linux");
1364 MODULE_LICENSE("GPL");
1365 MODULE_SUPPORTED_DEVICE("video");
1366