1 /*
2  * vpif-display - VPIF display driver
3  * Display driver for TI DaVinci VPIF
4  *
5  * Copyright (C) 2009 Texas Instruments Incorporated - https://www.ti.com/
6  * Copyright (C) 2014 Lad, Prabhakar <prabhakar.csengg@gmail.com>
7  *
8  * This program is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License as
10  * published by the Free Software Foundation version 2.
11  *
12  * This program is distributed .as is. WITHOUT ANY WARRANTY of any
13  * kind, whether express or implied; without even the implied warranty
14  * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15  * GNU General Public License for more details.
16  */
17 
18 #include <linux/interrupt.h>
19 #include <linux/module.h>
20 #include <linux/platform_device.h>
21 #include <linux/slab.h>
22 
23 #include <media/v4l2-ioctl.h>
24 
25 #include "vpif.h"
26 #include "vpif_display.h"
27 
28 MODULE_DESCRIPTION("TI DaVinci VPIF Display driver");
29 MODULE_LICENSE("GPL");
30 MODULE_VERSION(VPIF_DISPLAY_VERSION);
31 
32 #define VPIF_V4L2_STD (V4L2_STD_525_60 | V4L2_STD_625_50)
33 
34 #define vpif_err(fmt, arg...)	v4l2_err(&vpif_obj.v4l2_dev, fmt, ## arg)
35 #define vpif_dbg(level, debug, fmt, arg...)	\
36 		v4l2_dbg(level, debug, &vpif_obj.v4l2_dev, fmt, ## arg)
37 
38 static int debug = 1;
39 
40 module_param(debug, int, 0644);
41 
42 MODULE_PARM_DESC(debug, "Debug level 0-1");
43 
44 #define VPIF_DRIVER_NAME	"vpif_display"
45 MODULE_ALIAS("platform:" VPIF_DRIVER_NAME);
46 
47 /* Is set to 1 in case of SDTV formats, 2 in case of HDTV formats. */
48 static int ycmux_mode;
49 
50 static u8 channel_first_int[VPIF_NUMOBJECTS][2] = { {1, 1} };
51 
52 static struct vpif_device vpif_obj = { {NULL} };
53 static struct device *vpif_dev;
54 static void vpif_calculate_offsets(struct channel_obj *ch);
55 static void vpif_config_addr(struct channel_obj *ch, int muxmode);
56 
57 static inline
58 struct vpif_disp_buffer *to_vpif_buffer(struct vb2_v4l2_buffer *vb)
59 {
60 	return container_of(vb, struct vpif_disp_buffer, vb);
61 }
62 
63 /**
64  * vpif_buffer_prepare :  callback function for buffer prepare
65  * @vb: ptr to vb2_buffer
66  *
67  * This is the callback function for buffer prepare when vb2_qbuf()
68  * function is called. The buffer is prepared and user space virtual address
69  * or user address is converted into  physical address
70  */
71 static int vpif_buffer_prepare(struct vb2_buffer *vb)
72 {
73 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
74 	struct channel_obj *ch = vb2_get_drv_priv(vb->vb2_queue);
75 	struct common_obj *common;
76 
77 	common = &ch->common[VPIF_VIDEO_INDEX];
78 
79 	vb2_set_plane_payload(vb, 0, common->fmt.fmt.pix.sizeimage);
80 	if (vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0))
81 		return -EINVAL;
82 
83 	vbuf->field = common->fmt.fmt.pix.field;
84 
85 	if (vb->vb2_queue->type != V4L2_BUF_TYPE_SLICED_VBI_OUTPUT) {
86 		unsigned long addr = vb2_dma_contig_plane_dma_addr(vb, 0);
87 
88 		if (!ISALIGNED(addr + common->ytop_off) ||
89 			!ISALIGNED(addr + common->ybtm_off) ||
90 			!ISALIGNED(addr + common->ctop_off) ||
91 			!ISALIGNED(addr + common->cbtm_off)) {
92 			vpif_err("buffer offset not aligned to 8 bytes\n");
93 			return -EINVAL;
94 		}
95 	}
96 
97 	return 0;
98 }
99 
100 /**
101  * vpif_buffer_queue_setup : Callback function for buffer setup.
102  * @vq: vb2_queue ptr
103  * @nbuffers: ptr to number of buffers requested by application
104  * @nplanes: contains number of distinct video planes needed to hold a frame
105  * @sizes: contains the size (in bytes) of each plane.
106  * @alloc_devs: ptr to allocation context
107  *
108  * This callback function is called when reqbuf() is called to adjust
109  * the buffer count and buffer size
110  */
111 static int vpif_buffer_queue_setup(struct vb2_queue *vq,
112 				unsigned int *nbuffers, unsigned int *nplanes,
113 				unsigned int sizes[], struct device *alloc_devs[])
114 {
115 	struct channel_obj *ch = vb2_get_drv_priv(vq);
116 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
117 	unsigned size = common->fmt.fmt.pix.sizeimage;
118 
119 	if (*nplanes) {
120 		if (sizes[0] < size)
121 			return -EINVAL;
122 		size = sizes[0];
123 	}
124 
125 	if (vq->num_buffers + *nbuffers < 3)
126 		*nbuffers = 3 - vq->num_buffers;
127 
128 	*nplanes = 1;
129 	sizes[0] = size;
130 
131 	/* Calculate the offset for Y and C data  in the buffer */
132 	vpif_calculate_offsets(ch);
133 
134 	return 0;
135 }
136 
137 /**
138  * vpif_buffer_queue : Callback function to add buffer to DMA queue
139  * @vb: ptr to vb2_buffer
140  *
141  * This callback function queues the buffer to DMA engine
142  */
143 static void vpif_buffer_queue(struct vb2_buffer *vb)
144 {
145 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
146 	struct vpif_disp_buffer *buf = to_vpif_buffer(vbuf);
147 	struct channel_obj *ch = vb2_get_drv_priv(vb->vb2_queue);
148 	struct common_obj *common;
149 	unsigned long flags;
150 
151 	common = &ch->common[VPIF_VIDEO_INDEX];
152 
153 	/* add the buffer to the DMA queue */
154 	spin_lock_irqsave(&common->irqlock, flags);
155 	list_add_tail(&buf->list, &common->dma_queue);
156 	spin_unlock_irqrestore(&common->irqlock, flags);
157 }
158 
159 /**
160  * vpif_start_streaming : Starts the DMA engine for streaming
161  * @vq: ptr to vb2_buffer
162  * @count: number of buffers
163  */
164 static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
165 {
166 	struct vpif_display_config *vpif_config_data =
167 					vpif_dev->platform_data;
168 	struct channel_obj *ch = vb2_get_drv_priv(vq);
169 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
170 	struct vpif_params *vpif = &ch->vpifparams;
171 	struct vpif_disp_buffer *buf, *tmp;
172 	unsigned long addr, flags;
173 	int ret;
174 
175 	spin_lock_irqsave(&common->irqlock, flags);
176 
177 	/* Initialize field_id */
178 	ch->field_id = 0;
179 
180 	/* clock settings */
181 	if (vpif_config_data->set_clock) {
182 		ret = vpif_config_data->set_clock(ch->vpifparams.std_info.
183 		ycmux_mode, ch->vpifparams.std_info.hd_sd);
184 		if (ret < 0) {
185 			vpif_err("can't set clock\n");
186 			goto err;
187 		}
188 	}
189 
190 	/* set the parameters and addresses */
191 	ret = vpif_set_video_params(vpif, ch->channel_id + 2);
192 	if (ret < 0)
193 		goto err;
194 
195 	ycmux_mode = ret;
196 	vpif_config_addr(ch, ret);
197 	/* Get the next frame from the buffer queue */
198 	common->next_frm = common->cur_frm =
199 			    list_entry(common->dma_queue.next,
200 				       struct vpif_disp_buffer, list);
201 
202 	list_del(&common->cur_frm->list);
203 	spin_unlock_irqrestore(&common->irqlock, flags);
204 
205 	addr = vb2_dma_contig_plane_dma_addr(&common->cur_frm->vb.vb2_buf, 0);
206 	common->set_addr((addr + common->ytop_off),
207 			    (addr + common->ybtm_off),
208 			    (addr + common->ctop_off),
209 			    (addr + common->cbtm_off));
210 
211 	/*
212 	 * Set interrupt for both the fields in VPIF
213 	 * Register enable channel in VPIF register
214 	 */
215 	channel_first_int[VPIF_VIDEO_INDEX][ch->channel_id] = 1;
216 	if (VPIF_CHANNEL2_VIDEO == ch->channel_id) {
217 		channel2_intr_assert();
218 		channel2_intr_enable(1);
219 		enable_channel2(1);
220 		if (vpif_config_data->chan_config[VPIF_CHANNEL2_VIDEO].clip_en)
221 			channel2_clipping_enable(1);
222 	}
223 
224 	if (VPIF_CHANNEL3_VIDEO == ch->channel_id || ycmux_mode == 2) {
225 		channel3_intr_assert();
226 		channel3_intr_enable(1);
227 		enable_channel3(1);
228 		if (vpif_config_data->chan_config[VPIF_CHANNEL3_VIDEO].clip_en)
229 			channel3_clipping_enable(1);
230 	}
231 
232 	return 0;
233 
234 err:
235 	list_for_each_entry_safe(buf, tmp, &common->dma_queue, list) {
236 		list_del(&buf->list);
237 		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
238 	}
239 	spin_unlock_irqrestore(&common->irqlock, flags);
240 
241 	return ret;
242 }
243 
244 /**
245  * vpif_stop_streaming : Stop the DMA engine
246  * @vq: ptr to vb2_queue
247  *
248  * This callback stops the DMA engine and any remaining buffers
249  * in the DMA queue are released.
250  */
251 static void vpif_stop_streaming(struct vb2_queue *vq)
252 {
253 	struct channel_obj *ch = vb2_get_drv_priv(vq);
254 	struct common_obj *common;
255 	unsigned long flags;
256 
257 	common = &ch->common[VPIF_VIDEO_INDEX];
258 
259 	/* Disable channel */
260 	if (VPIF_CHANNEL2_VIDEO == ch->channel_id) {
261 		enable_channel2(0);
262 		channel2_intr_enable(0);
263 	}
264 	if (VPIF_CHANNEL3_VIDEO == ch->channel_id || ycmux_mode == 2) {
265 		enable_channel3(0);
266 		channel3_intr_enable(0);
267 	}
268 
269 	/* release all active buffers */
270 	spin_lock_irqsave(&common->irqlock, flags);
271 	if (common->cur_frm == common->next_frm) {
272 		vb2_buffer_done(&common->cur_frm->vb.vb2_buf,
273 				VB2_BUF_STATE_ERROR);
274 	} else {
275 		if (common->cur_frm)
276 			vb2_buffer_done(&common->cur_frm->vb.vb2_buf,
277 					VB2_BUF_STATE_ERROR);
278 		if (common->next_frm)
279 			vb2_buffer_done(&common->next_frm->vb.vb2_buf,
280 					VB2_BUF_STATE_ERROR);
281 	}
282 
283 	while (!list_empty(&common->dma_queue)) {
284 		common->next_frm = list_entry(common->dma_queue.next,
285 						struct vpif_disp_buffer, list);
286 		list_del(&common->next_frm->list);
287 		vb2_buffer_done(&common->next_frm->vb.vb2_buf,
288 				VB2_BUF_STATE_ERROR);
289 	}
290 	spin_unlock_irqrestore(&common->irqlock, flags);
291 }
292 
293 static const struct vb2_ops video_qops = {
294 	.queue_setup		= vpif_buffer_queue_setup,
295 	.wait_prepare		= vb2_ops_wait_prepare,
296 	.wait_finish		= vb2_ops_wait_finish,
297 	.buf_prepare		= vpif_buffer_prepare,
298 	.start_streaming	= vpif_start_streaming,
299 	.stop_streaming		= vpif_stop_streaming,
300 	.buf_queue		= vpif_buffer_queue,
301 };
302 
303 static void process_progressive_mode(struct common_obj *common)
304 {
305 	unsigned long addr;
306 
307 	spin_lock(&common->irqlock);
308 	/* Get the next buffer from buffer queue */
309 	common->next_frm = list_entry(common->dma_queue.next,
310 				struct vpif_disp_buffer, list);
311 	/* Remove that buffer from the buffer queue */
312 	list_del(&common->next_frm->list);
313 	spin_unlock(&common->irqlock);
314 
315 	/* Set top and bottom field addrs in VPIF registers */
316 	addr = vb2_dma_contig_plane_dma_addr(&common->next_frm->vb.vb2_buf, 0);
317 	common->set_addr(addr + common->ytop_off,
318 				 addr + common->ybtm_off,
319 				 addr + common->ctop_off,
320 				 addr + common->cbtm_off);
321 }
322 
323 static void process_interlaced_mode(int fid, struct common_obj *common)
324 {
325 	/* device field id and local field id are in sync */
326 	/* If this is even field */
327 	if (0 == fid) {
328 		if (common->cur_frm == common->next_frm)
329 			return;
330 
331 		/* one frame is displayed If next frame is
332 		 *  available, release cur_frm and move on */
333 		/* Copy frame display time */
334 		common->cur_frm->vb.vb2_buf.timestamp = ktime_get_ns();
335 		/* Change status of the cur_frm */
336 		vb2_buffer_done(&common->cur_frm->vb.vb2_buf,
337 					VB2_BUF_STATE_DONE);
338 		/* Make cur_frm pointing to next_frm */
339 		common->cur_frm = common->next_frm;
340 
341 	} else if (1 == fid) {	/* odd field */
342 		spin_lock(&common->irqlock);
343 		if (list_empty(&common->dma_queue)
344 		    || (common->cur_frm != common->next_frm)) {
345 			spin_unlock(&common->irqlock);
346 			return;
347 		}
348 		spin_unlock(&common->irqlock);
349 		/* one field is displayed configure the next
350 		 * frame if it is available else hold on current
351 		 * frame */
352 		/* Get next from the buffer queue */
353 		process_progressive_mode(common);
354 	}
355 }
356 
357 /*
358  * vpif_channel_isr: It changes status of the displayed buffer, takes next
359  * buffer from the queue and sets its address in VPIF registers
360  */
361 static irqreturn_t vpif_channel_isr(int irq, void *dev_id)
362 {
363 	struct vpif_device *dev = &vpif_obj;
364 	struct channel_obj *ch;
365 	struct common_obj *common;
366 	int fid = -1, i;
367 	int channel_id;
368 
369 	channel_id = *(int *)(dev_id);
370 	if (!vpif_intr_status(channel_id + 2))
371 		return IRQ_NONE;
372 
373 	ch = dev->dev[channel_id];
374 	for (i = 0; i < VPIF_NUMOBJECTS; i++) {
375 		common = &ch->common[i];
376 		/* If streaming is started in this channel */
377 
378 		if (1 == ch->vpifparams.std_info.frm_fmt) {
379 			spin_lock(&common->irqlock);
380 			if (list_empty(&common->dma_queue)) {
381 				spin_unlock(&common->irqlock);
382 				continue;
383 			}
384 			spin_unlock(&common->irqlock);
385 
386 			/* Progressive mode */
387 			if (!channel_first_int[i][channel_id]) {
388 				/* Mark status of the cur_frm to
389 				 * done and unlock semaphore on it */
390 				common->cur_frm->vb.vb2_buf.timestamp =
391 						ktime_get_ns();
392 				vb2_buffer_done(&common->cur_frm->vb.vb2_buf,
393 						VB2_BUF_STATE_DONE);
394 				/* Make cur_frm pointing to next_frm */
395 				common->cur_frm = common->next_frm;
396 			}
397 
398 			channel_first_int[i][channel_id] = 0;
399 			process_progressive_mode(common);
400 		} else {
401 			/* Interlaced mode */
402 			/* If it is first interrupt, ignore it */
403 
404 			if (channel_first_int[i][channel_id]) {
405 				channel_first_int[i][channel_id] = 0;
406 				continue;
407 			}
408 
409 			if (0 == i) {
410 				ch->field_id ^= 1;
411 				/* Get field id from VPIF registers */
412 				fid = vpif_channel_getfid(ch->channel_id + 2);
413 				/* If fid does not match with stored field id */
414 				if (fid != ch->field_id) {
415 					/* Make them in sync */
416 					if (0 == fid)
417 						ch->field_id = fid;
418 
419 					return IRQ_HANDLED;
420 				}
421 			}
422 			process_interlaced_mode(fid, common);
423 		}
424 	}
425 
426 	return IRQ_HANDLED;
427 }
428 
429 static int vpif_update_std_info(struct channel_obj *ch)
430 {
431 	struct video_obj *vid_ch = &ch->video;
432 	struct vpif_params *vpifparams = &ch->vpifparams;
433 	struct vpif_channel_config_params *std_info = &vpifparams->std_info;
434 	const struct vpif_channel_config_params *config;
435 
436 	int i;
437 
438 	for (i = 0; i < vpif_ch_params_count; i++) {
439 		config = &vpif_ch_params[i];
440 		if (config->hd_sd == 0) {
441 			vpif_dbg(2, debug, "SD format\n");
442 			if (config->stdid & vid_ch->stdid) {
443 				memcpy(std_info, config, sizeof(*config));
444 				break;
445 			}
446 		}
447 	}
448 
449 	if (i == vpif_ch_params_count) {
450 		vpif_dbg(1, debug, "Format not found\n");
451 		return -EINVAL;
452 	}
453 
454 	return 0;
455 }
456 
457 static int vpif_update_resolution(struct channel_obj *ch)
458 {
459 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
460 	struct video_obj *vid_ch = &ch->video;
461 	struct vpif_params *vpifparams = &ch->vpifparams;
462 	struct vpif_channel_config_params *std_info = &vpifparams->std_info;
463 
464 	if (!vid_ch->stdid && !vid_ch->dv_timings.bt.height)
465 		return -EINVAL;
466 
467 	if (vid_ch->stdid) {
468 		if (vpif_update_std_info(ch))
469 			return -EINVAL;
470 	}
471 
472 	common->fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV422P;
473 	common->fmt.fmt.pix.width = std_info->width;
474 	common->fmt.fmt.pix.height = std_info->height;
475 	vpif_dbg(1, debug, "Pixel details: Width = %d,Height = %d\n",
476 			common->fmt.fmt.pix.width, common->fmt.fmt.pix.height);
477 
478 	/* Set height and width paramateres */
479 	common->height = std_info->height;
480 	common->width = std_info->width;
481 	common->fmt.fmt.pix.sizeimage = common->height * common->width * 2;
482 
483 	if (vid_ch->stdid)
484 		common->fmt.fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
485 	else
486 		common->fmt.fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
487 
488 	if (ch->vpifparams.std_info.frm_fmt)
489 		common->fmt.fmt.pix.field = V4L2_FIELD_NONE;
490 	else
491 		common->fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
492 
493 	return 0;
494 }
495 
496 /*
497  * vpif_calculate_offsets: This function calculates buffers offset for Y and C
498  * in the top and bottom field
499  */
500 static void vpif_calculate_offsets(struct channel_obj *ch)
501 {
502 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
503 	struct vpif_params *vpifparams = &ch->vpifparams;
504 	enum v4l2_field field = common->fmt.fmt.pix.field;
505 	struct video_obj *vid_ch = &ch->video;
506 	unsigned int hpitch, sizeimage;
507 
508 	if (V4L2_FIELD_ANY == common->fmt.fmt.pix.field) {
509 		if (ch->vpifparams.std_info.frm_fmt)
510 			vid_ch->buf_field = V4L2_FIELD_NONE;
511 		else
512 			vid_ch->buf_field = V4L2_FIELD_INTERLACED;
513 	} else {
514 		vid_ch->buf_field = common->fmt.fmt.pix.field;
515 	}
516 
517 	sizeimage = common->fmt.fmt.pix.sizeimage;
518 
519 	hpitch = common->fmt.fmt.pix.bytesperline;
520 	if ((V4L2_FIELD_NONE == vid_ch->buf_field) ||
521 	    (V4L2_FIELD_INTERLACED == vid_ch->buf_field)) {
522 		common->ytop_off = 0;
523 		common->ybtm_off = hpitch;
524 		common->ctop_off = sizeimage / 2;
525 		common->cbtm_off = sizeimage / 2 + hpitch;
526 	} else if (V4L2_FIELD_SEQ_TB == vid_ch->buf_field) {
527 		common->ytop_off = 0;
528 		common->ybtm_off = sizeimage / 4;
529 		common->ctop_off = sizeimage / 2;
530 		common->cbtm_off = common->ctop_off + sizeimage / 4;
531 	} else if (V4L2_FIELD_SEQ_BT == vid_ch->buf_field) {
532 		common->ybtm_off = 0;
533 		common->ytop_off = sizeimage / 4;
534 		common->cbtm_off = sizeimage / 2;
535 		common->ctop_off = common->cbtm_off + sizeimage / 4;
536 	}
537 
538 	if ((V4L2_FIELD_NONE == vid_ch->buf_field) ||
539 	    (V4L2_FIELD_INTERLACED == vid_ch->buf_field)) {
540 		vpifparams->video_params.storage_mode = 1;
541 	} else {
542 		vpifparams->video_params.storage_mode = 0;
543 	}
544 
545 	if (ch->vpifparams.std_info.frm_fmt == 1) {
546 		vpifparams->video_params.hpitch =
547 		    common->fmt.fmt.pix.bytesperline;
548 	} else {
549 		if ((field == V4L2_FIELD_ANY) ||
550 			(field == V4L2_FIELD_INTERLACED))
551 			vpifparams->video_params.hpitch =
552 			    common->fmt.fmt.pix.bytesperline * 2;
553 		else
554 			vpifparams->video_params.hpitch =
555 			    common->fmt.fmt.pix.bytesperline;
556 	}
557 
558 	ch->vpifparams.video_params.stdid = ch->vpifparams.std_info.stdid;
559 }
560 
561 static void vpif_config_addr(struct channel_obj *ch, int muxmode)
562 {
563 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
564 
565 	if (VPIF_CHANNEL3_VIDEO == ch->channel_id) {
566 		common->set_addr = ch3_set_video_buf_addr;
567 	} else {
568 		if (2 == muxmode)
569 			common->set_addr = ch2_set_video_buf_addr_yc_nmux;
570 		else
571 			common->set_addr = ch2_set_video_buf_addr;
572 	}
573 }
574 
575 /* functions implementing ioctls */
576 /**
577  * vpif_querycap() - QUERYCAP handler
578  * @file: file ptr
579  * @priv: file handle
580  * @cap: ptr to v4l2_capability structure
581  */
582 static int vpif_querycap(struct file *file, void  *priv,
583 				struct v4l2_capability *cap)
584 {
585 	struct vpif_display_config *config = vpif_dev->platform_data;
586 
587 	strscpy(cap->driver, VPIF_DRIVER_NAME, sizeof(cap->driver));
588 	strscpy(cap->card, config->card_name, sizeof(cap->card));
589 
590 	return 0;
591 }
592 
593 static int vpif_enum_fmt_vid_out(struct file *file, void  *priv,
594 					struct v4l2_fmtdesc *fmt)
595 {
596 	if (fmt->index != 0)
597 		return -EINVAL;
598 
599 	/* Fill in the information about format */
600 	fmt->pixelformat = V4L2_PIX_FMT_YUV422P;
601 	return 0;
602 }
603 
604 static int vpif_g_fmt_vid_out(struct file *file, void *priv,
605 				struct v4l2_format *fmt)
606 {
607 	struct video_device *vdev = video_devdata(file);
608 	struct channel_obj *ch = video_get_drvdata(vdev);
609 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
610 
611 	/* Check the validity of the buffer type */
612 	if (common->fmt.type != fmt->type)
613 		return -EINVAL;
614 
615 	if (vpif_update_resolution(ch))
616 		return -EINVAL;
617 	*fmt = common->fmt;
618 	return 0;
619 }
620 
621 static int vpif_try_fmt_vid_out(struct file *file, void *priv,
622 				struct v4l2_format *fmt)
623 {
624 	struct video_device *vdev = video_devdata(file);
625 	struct channel_obj *ch = video_get_drvdata(vdev);
626 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
627 	struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
628 
629 	/*
630 	 * to suppress v4l-compliance warnings silently correct
631 	 * the pixelformat
632 	 */
633 	if (pixfmt->pixelformat != V4L2_PIX_FMT_YUV422P)
634 		pixfmt->pixelformat = common->fmt.fmt.pix.pixelformat;
635 
636 	if (vpif_update_resolution(ch))
637 		return -EINVAL;
638 
639 	pixfmt->colorspace = common->fmt.fmt.pix.colorspace;
640 	pixfmt->field = common->fmt.fmt.pix.field;
641 	pixfmt->bytesperline = common->fmt.fmt.pix.width;
642 	pixfmt->width = common->fmt.fmt.pix.width;
643 	pixfmt->height = common->fmt.fmt.pix.height;
644 	pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height * 2;
645 
646 	return 0;
647 }
648 
649 static int vpif_s_fmt_vid_out(struct file *file, void *priv,
650 				struct v4l2_format *fmt)
651 {
652 	struct video_device *vdev = video_devdata(file);
653 	struct channel_obj *ch = video_get_drvdata(vdev);
654 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
655 	struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
656 	int ret;
657 
658 	if (vb2_is_busy(&common->buffer_queue))
659 		return -EBUSY;
660 
661 	ret = vpif_try_fmt_vid_out(file, priv, fmt);
662 	if (ret)
663 		return ret;
664 
665 	/* store the pix format in the channel object */
666 	common->fmt.fmt.pix = *pixfmt;
667 
668 	/* store the format in the channel object */
669 	common->fmt = *fmt;
670 	return 0;
671 }
672 
673 static int vpif_s_std(struct file *file, void *priv, v4l2_std_id std_id)
674 {
675 	struct vpif_display_config *config = vpif_dev->platform_data;
676 	struct video_device *vdev = video_devdata(file);
677 	struct channel_obj *ch = video_get_drvdata(vdev);
678 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
679 	struct vpif_display_chan_config *chan_cfg;
680 	struct v4l2_output output;
681 	int ret;
682 
683 	if (!config->chan_config[ch->channel_id].outputs)
684 		return -ENODATA;
685 
686 	chan_cfg = &config->chan_config[ch->channel_id];
687 	output = chan_cfg->outputs[ch->output_idx].output;
688 	if (output.capabilities != V4L2_OUT_CAP_STD)
689 		return -ENODATA;
690 
691 	if (vb2_is_busy(&common->buffer_queue))
692 		return -EBUSY;
693 
694 
695 	if (!(std_id & VPIF_V4L2_STD))
696 		return -EINVAL;
697 
698 	/* Call encoder subdevice function to set the standard */
699 	ch->video.stdid = std_id;
700 	memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings));
701 	/* Get the information about the standard */
702 	if (vpif_update_resolution(ch))
703 		return -EINVAL;
704 
705 	common->fmt.fmt.pix.bytesperline = common->fmt.fmt.pix.width;
706 
707 	ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, video,
708 						s_std_output, std_id);
709 	if (ret < 0) {
710 		vpif_err("Failed to set output standard\n");
711 		return ret;
712 	}
713 
714 	ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, video,
715 							s_std, std_id);
716 	if (ret < 0)
717 		vpif_err("Failed to set standard for sub devices\n");
718 	return ret;
719 }
720 
721 static int vpif_g_std(struct file *file, void *priv, v4l2_std_id *std)
722 {
723 	struct vpif_display_config *config = vpif_dev->platform_data;
724 	struct video_device *vdev = video_devdata(file);
725 	struct channel_obj *ch = video_get_drvdata(vdev);
726 	struct vpif_display_chan_config *chan_cfg;
727 	struct v4l2_output output;
728 
729 	if (!config->chan_config[ch->channel_id].outputs)
730 		return -ENODATA;
731 
732 	chan_cfg = &config->chan_config[ch->channel_id];
733 	output = chan_cfg->outputs[ch->output_idx].output;
734 	if (output.capabilities != V4L2_OUT_CAP_STD)
735 		return -ENODATA;
736 
737 	*std = ch->video.stdid;
738 	return 0;
739 }
740 
741 static int vpif_enum_output(struct file *file, void *fh,
742 				struct v4l2_output *output)
743 {
744 
745 	struct vpif_display_config *config = vpif_dev->platform_data;
746 	struct video_device *vdev = video_devdata(file);
747 	struct channel_obj *ch = video_get_drvdata(vdev);
748 	struct vpif_display_chan_config *chan_cfg;
749 
750 	chan_cfg = &config->chan_config[ch->channel_id];
751 	if (output->index >= chan_cfg->output_count) {
752 		vpif_dbg(1, debug, "Invalid output index\n");
753 		return -EINVAL;
754 	}
755 
756 	*output = chan_cfg->outputs[output->index].output;
757 	return 0;
758 }
759 
760 /**
761  * vpif_output_to_subdev() - Maps output to sub device
762  * @vpif_cfg: global config ptr
763  * @chan_cfg: channel config ptr
764  * @index: Given output index from application
765  *
766  * lookup the sub device information for a given output index.
767  * we report all the output to application. output table also
768  * has sub device name for the each output
769  */
770 static int
771 vpif_output_to_subdev(struct vpif_display_config *vpif_cfg,
772 		      struct vpif_display_chan_config *chan_cfg, int index)
773 {
774 	struct vpif_subdev_info *subdev_info;
775 	const char *subdev_name;
776 	int i;
777 
778 	vpif_dbg(2, debug, "vpif_output_to_subdev\n");
779 
780 	if (!chan_cfg->outputs)
781 		return -1;
782 
783 	subdev_name = chan_cfg->outputs[index].subdev_name;
784 	if (!subdev_name)
785 		return -1;
786 
787 	/* loop through the sub device list to get the sub device info */
788 	for (i = 0; i < vpif_cfg->subdev_count; i++) {
789 		subdev_info = &vpif_cfg->subdevinfo[i];
790 		if (!strcmp(subdev_info->name, subdev_name))
791 			return i;
792 	}
793 	return -1;
794 }
795 
796 /**
797  * vpif_set_output() - Select an output
798  * @vpif_cfg: global config ptr
799  * @ch: channel
800  * @index: Given output index from application
801  *
802  * Select the given output.
803  */
804 static int vpif_set_output(struct vpif_display_config *vpif_cfg,
805 		      struct channel_obj *ch, int index)
806 {
807 	struct vpif_display_chan_config *chan_cfg =
808 		&vpif_cfg->chan_config[ch->channel_id];
809 	struct v4l2_subdev *sd = NULL;
810 	u32 input = 0, output = 0;
811 	int sd_index;
812 	int ret;
813 
814 	sd_index = vpif_output_to_subdev(vpif_cfg, chan_cfg, index);
815 	if (sd_index >= 0)
816 		sd = vpif_obj.sd[sd_index];
817 
818 	if (sd) {
819 		input = chan_cfg->outputs[index].input_route;
820 		output = chan_cfg->outputs[index].output_route;
821 		ret = v4l2_subdev_call(sd, video, s_routing, input, output, 0);
822 		if (ret < 0 && ret != -ENOIOCTLCMD) {
823 			vpif_err("Failed to set output\n");
824 			return ret;
825 		}
826 
827 	}
828 	ch->output_idx = index;
829 	ch->sd = sd;
830 	if (chan_cfg->outputs)
831 		/* update tvnorms from the sub device output info */
832 		ch->video_dev.tvnorms = chan_cfg->outputs[index].output.std;
833 	return 0;
834 }
835 
836 static int vpif_s_output(struct file *file, void *priv, unsigned int i)
837 {
838 	struct vpif_display_config *config = vpif_dev->platform_data;
839 	struct video_device *vdev = video_devdata(file);
840 	struct channel_obj *ch = video_get_drvdata(vdev);
841 	struct vpif_display_chan_config *chan_cfg;
842 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
843 
844 	if (vb2_is_busy(&common->buffer_queue))
845 		return -EBUSY;
846 
847 	chan_cfg = &config->chan_config[ch->channel_id];
848 
849 	if (i >= chan_cfg->output_count)
850 		return -EINVAL;
851 
852 	return vpif_set_output(config, ch, i);
853 }
854 
855 static int vpif_g_output(struct file *file, void *priv, unsigned int *i)
856 {
857 	struct video_device *vdev = video_devdata(file);
858 	struct channel_obj *ch = video_get_drvdata(vdev);
859 
860 	*i = ch->output_idx;
861 
862 	return 0;
863 }
864 
865 /**
866  * vpif_enum_dv_timings() - ENUM_DV_TIMINGS handler
867  * @file: file ptr
868  * @priv: file handle
869  * @timings: input timings
870  */
871 static int
872 vpif_enum_dv_timings(struct file *file, void *priv,
873 		     struct v4l2_enum_dv_timings *timings)
874 {
875 	struct vpif_display_config *config = vpif_dev->platform_data;
876 	struct video_device *vdev = video_devdata(file);
877 	struct channel_obj *ch = video_get_drvdata(vdev);
878 	struct vpif_display_chan_config *chan_cfg;
879 	struct v4l2_output output;
880 	int ret;
881 
882 	if (!config->chan_config[ch->channel_id].outputs)
883 		return -ENODATA;
884 
885 	chan_cfg = &config->chan_config[ch->channel_id];
886 	output = chan_cfg->outputs[ch->output_idx].output;
887 	if (output.capabilities != V4L2_OUT_CAP_DV_TIMINGS)
888 		return -ENODATA;
889 
890 	timings->pad = 0;
891 
892 	ret = v4l2_subdev_call(ch->sd, pad, enum_dv_timings, timings);
893 	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
894 		return -EINVAL;
895 	return ret;
896 }
897 
898 /**
899  * vpif_s_dv_timings() - S_DV_TIMINGS handler
900  * @file: file ptr
901  * @priv: file handle
902  * @timings: digital video timings
903  */
904 static int vpif_s_dv_timings(struct file *file, void *priv,
905 		struct v4l2_dv_timings *timings)
906 {
907 	struct vpif_display_config *config = vpif_dev->platform_data;
908 	struct video_device *vdev = video_devdata(file);
909 	struct channel_obj *ch = video_get_drvdata(vdev);
910 	struct vpif_params *vpifparams = &ch->vpifparams;
911 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
912 	struct vpif_channel_config_params *std_info = &vpifparams->std_info;
913 	struct video_obj *vid_ch = &ch->video;
914 	struct v4l2_bt_timings *bt = &vid_ch->dv_timings.bt;
915 	struct vpif_display_chan_config *chan_cfg;
916 	struct v4l2_output output;
917 	int ret;
918 
919 	if (!config->chan_config[ch->channel_id].outputs)
920 		return -ENODATA;
921 
922 	chan_cfg = &config->chan_config[ch->channel_id];
923 	output = chan_cfg->outputs[ch->output_idx].output;
924 	if (output.capabilities != V4L2_OUT_CAP_DV_TIMINGS)
925 		return -ENODATA;
926 
927 	if (vb2_is_busy(&common->buffer_queue))
928 		return -EBUSY;
929 
930 	if (timings->type != V4L2_DV_BT_656_1120) {
931 		vpif_dbg(2, debug, "Timing type not defined\n");
932 		return -EINVAL;
933 	}
934 
935 	/* Configure subdevice timings, if any */
936 	ret = v4l2_subdev_call(ch->sd, video, s_dv_timings, timings);
937 	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
938 		ret = 0;
939 	if (ret < 0) {
940 		vpif_dbg(2, debug, "Error setting custom DV timings\n");
941 		return ret;
942 	}
943 
944 	if (!(timings->bt.width && timings->bt.height &&
945 				(timings->bt.hbackporch ||
946 				 timings->bt.hfrontporch ||
947 				 timings->bt.hsync) &&
948 				timings->bt.vfrontporch &&
949 				(timings->bt.vbackporch ||
950 				 timings->bt.vsync))) {
951 		vpif_dbg(2, debug, "Timings for width, height, horizontal back porch, horizontal sync, horizontal front porch, vertical back porch, vertical sync and vertical back porch must be defined\n");
952 		return -EINVAL;
953 	}
954 
955 	vid_ch->dv_timings = *timings;
956 
957 	/* Configure video port timings */
958 
959 	std_info->eav2sav = V4L2_DV_BT_BLANKING_WIDTH(bt) - 8;
960 	std_info->sav2eav = bt->width;
961 
962 	std_info->l1 = 1;
963 	std_info->l3 = bt->vsync + bt->vbackporch + 1;
964 
965 	std_info->vsize = V4L2_DV_BT_FRAME_HEIGHT(bt);
966 	if (bt->interlaced) {
967 		if (bt->il_vbackporch || bt->il_vfrontporch || bt->il_vsync) {
968 			std_info->l5 = std_info->vsize/2 -
969 				(bt->vfrontporch - 1);
970 			std_info->l7 = std_info->vsize/2 + 1;
971 			std_info->l9 = std_info->l7 + bt->il_vsync +
972 				bt->il_vbackporch + 1;
973 			std_info->l11 = std_info->vsize -
974 				(bt->il_vfrontporch - 1);
975 		} else {
976 			vpif_dbg(2, debug, "Required timing values for interlaced BT format missing\n");
977 			return -EINVAL;
978 		}
979 	} else {
980 		std_info->l5 = std_info->vsize - (bt->vfrontporch - 1);
981 	}
982 	strscpy(std_info->name, "Custom timings BT656/1120",
983 		sizeof(std_info->name));
984 	std_info->width = bt->width;
985 	std_info->height = bt->height;
986 	std_info->frm_fmt = bt->interlaced ? 0 : 1;
987 	std_info->ycmux_mode = 0;
988 	std_info->capture_format = 0;
989 	std_info->vbi_supported = 0;
990 	std_info->hd_sd = 1;
991 	std_info->stdid = 0;
992 	vid_ch->stdid = 0;
993 
994 	return 0;
995 }
996 
997 /**
998  * vpif_g_dv_timings() - G_DV_TIMINGS handler
999  * @file: file ptr
1000  * @priv: file handle
1001  * @timings: digital video timings
1002  */
1003 static int vpif_g_dv_timings(struct file *file, void *priv,
1004 		struct v4l2_dv_timings *timings)
1005 {
1006 	struct vpif_display_config *config = vpif_dev->platform_data;
1007 	struct video_device *vdev = video_devdata(file);
1008 	struct channel_obj *ch = video_get_drvdata(vdev);
1009 	struct vpif_display_chan_config *chan_cfg;
1010 	struct video_obj *vid_ch = &ch->video;
1011 	struct v4l2_output output;
1012 
1013 	if (!config->chan_config[ch->channel_id].outputs)
1014 		goto error;
1015 
1016 	chan_cfg = &config->chan_config[ch->channel_id];
1017 	output = chan_cfg->outputs[ch->output_idx].output;
1018 
1019 	if (output.capabilities != V4L2_OUT_CAP_DV_TIMINGS)
1020 		goto error;
1021 
1022 	*timings = vid_ch->dv_timings;
1023 
1024 	return 0;
1025 error:
1026 	return -ENODATA;
1027 }
1028 
1029 /*
1030  * vpif_log_status() - Status information
1031  * @file: file ptr
1032  * @priv: file handle
1033  *
1034  * Returns zero.
1035  */
1036 static int vpif_log_status(struct file *filep, void *priv)
1037 {
1038 	/* status for sub devices */
1039 	v4l2_device_call_all(&vpif_obj.v4l2_dev, 0, core, log_status);
1040 
1041 	return 0;
1042 }
1043 
1044 /* vpif display ioctl operations */
1045 static const struct v4l2_ioctl_ops vpif_ioctl_ops = {
1046 	.vidioc_querycap		= vpif_querycap,
1047 	.vidioc_enum_fmt_vid_out	= vpif_enum_fmt_vid_out,
1048 	.vidioc_g_fmt_vid_out		= vpif_g_fmt_vid_out,
1049 	.vidioc_s_fmt_vid_out		= vpif_s_fmt_vid_out,
1050 	.vidioc_try_fmt_vid_out		= vpif_try_fmt_vid_out,
1051 
1052 	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
1053 	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
1054 	.vidioc_querybuf		= vb2_ioctl_querybuf,
1055 	.vidioc_qbuf			= vb2_ioctl_qbuf,
1056 	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
1057 	.vidioc_expbuf			= vb2_ioctl_expbuf,
1058 	.vidioc_streamon		= vb2_ioctl_streamon,
1059 	.vidioc_streamoff		= vb2_ioctl_streamoff,
1060 
1061 	.vidioc_s_std			= vpif_s_std,
1062 	.vidioc_g_std			= vpif_g_std,
1063 
1064 	.vidioc_enum_output		= vpif_enum_output,
1065 	.vidioc_s_output		= vpif_s_output,
1066 	.vidioc_g_output		= vpif_g_output,
1067 
1068 	.vidioc_enum_dv_timings		= vpif_enum_dv_timings,
1069 	.vidioc_s_dv_timings		= vpif_s_dv_timings,
1070 	.vidioc_g_dv_timings		= vpif_g_dv_timings,
1071 
1072 	.vidioc_log_status		= vpif_log_status,
1073 };
1074 
1075 static const struct v4l2_file_operations vpif_fops = {
1076 	.owner		= THIS_MODULE,
1077 	.open		= v4l2_fh_open,
1078 	.release	= vb2_fop_release,
1079 	.unlocked_ioctl	= video_ioctl2,
1080 	.mmap		= vb2_fop_mmap,
1081 	.poll		= vb2_fop_poll
1082 };
1083 
1084 /*Configure the channels, buffer sizei, request irq */
1085 static int initialize_vpif(void)
1086 {
1087 	int free_channel_objects_index;
1088 	int err, i, j;
1089 
1090 	/* Allocate memory for six channel objects */
1091 	for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
1092 		vpif_obj.dev[i] =
1093 		    kzalloc(sizeof(struct channel_obj), GFP_KERNEL);
1094 		/* If memory allocation fails, return error */
1095 		if (!vpif_obj.dev[i]) {
1096 			free_channel_objects_index = i;
1097 			err = -ENOMEM;
1098 			goto vpif_init_free_channel_objects;
1099 		}
1100 	}
1101 
1102 	return 0;
1103 
1104 vpif_init_free_channel_objects:
1105 	for (j = 0; j < free_channel_objects_index; j++)
1106 		kfree(vpif_obj.dev[j]);
1107 	return err;
1108 }
1109 
1110 static void free_vpif_objs(void)
1111 {
1112 	int i;
1113 
1114 	for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++)
1115 		kfree(vpif_obj.dev[i]);
1116 }
1117 
1118 static int vpif_probe_complete(void)
1119 {
1120 	struct common_obj *common;
1121 	struct video_device *vdev;
1122 	struct channel_obj *ch;
1123 	struct vb2_queue *q;
1124 	int j, err, k;
1125 
1126 	for (j = 0; j < VPIF_DISPLAY_MAX_DEVICES; j++) {
1127 		ch = vpif_obj.dev[j];
1128 		/* Initialize field of the channel objects */
1129 		for (k = 0; k < VPIF_NUMOBJECTS; k++) {
1130 			common = &ch->common[k];
1131 			spin_lock_init(&common->irqlock);
1132 			mutex_init(&common->lock);
1133 			common->set_addr = NULL;
1134 			common->ytop_off = 0;
1135 			common->ybtm_off = 0;
1136 			common->ctop_off = 0;
1137 			common->cbtm_off = 0;
1138 			common->cur_frm = NULL;
1139 			common->next_frm = NULL;
1140 			memset(&common->fmt, 0, sizeof(common->fmt));
1141 		}
1142 		ch->initialized = 0;
1143 		if (vpif_obj.config->subdev_count)
1144 			ch->sd = vpif_obj.sd[0];
1145 		ch->channel_id = j;
1146 
1147 		memset(&ch->vpifparams, 0, sizeof(ch->vpifparams));
1148 
1149 		ch->common[VPIF_VIDEO_INDEX].fmt.type =
1150 						V4L2_BUF_TYPE_VIDEO_OUTPUT;
1151 
1152 		/* select output 0 */
1153 		err = vpif_set_output(vpif_obj.config, ch, 0);
1154 		if (err)
1155 			goto probe_out;
1156 
1157 		/* set initial format */
1158 		ch->video.stdid = V4L2_STD_525_60;
1159 		memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings));
1160 		vpif_update_resolution(ch);
1161 
1162 		/* Initialize vb2 queue */
1163 		q = &common->buffer_queue;
1164 		q->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
1165 		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
1166 		q->drv_priv = ch;
1167 		q->ops = &video_qops;
1168 		q->mem_ops = &vb2_dma_contig_memops;
1169 		q->buf_struct_size = sizeof(struct vpif_disp_buffer);
1170 		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1171 		q->min_buffers_needed = 1;
1172 		q->lock = &common->lock;
1173 		q->dev = vpif_dev;
1174 		err = vb2_queue_init(q);
1175 		if (err) {
1176 			vpif_err("vpif_display: vb2_queue_init() failed\n");
1177 			goto probe_out;
1178 		}
1179 
1180 		INIT_LIST_HEAD(&common->dma_queue);
1181 
1182 		/* register video device */
1183 		vpif_dbg(1, debug, "channel=%p,channel->video_dev=%p\n",
1184 			 ch, &ch->video_dev);
1185 
1186 		/* Initialize the video_device structure */
1187 		vdev = &ch->video_dev;
1188 		strscpy(vdev->name, VPIF_DRIVER_NAME, sizeof(vdev->name));
1189 		vdev->release = video_device_release_empty;
1190 		vdev->fops = &vpif_fops;
1191 		vdev->ioctl_ops = &vpif_ioctl_ops;
1192 		vdev->v4l2_dev = &vpif_obj.v4l2_dev;
1193 		vdev->vfl_dir = VFL_DIR_TX;
1194 		vdev->queue = q;
1195 		vdev->lock = &common->lock;
1196 		vdev->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
1197 		video_set_drvdata(&ch->video_dev, ch);
1198 		err = video_register_device(vdev, VFL_TYPE_VIDEO,
1199 					    (j ? 3 : 2));
1200 		if (err < 0)
1201 			goto probe_out;
1202 	}
1203 
1204 	return 0;
1205 
1206 probe_out:
1207 	for (k = 0; k < j; k++) {
1208 		ch = vpif_obj.dev[k];
1209 		video_unregister_device(&ch->video_dev);
1210 	}
1211 	return err;
1212 }
1213 
1214 /*
1215  * vpif_probe: This function creates device entries by register itself to the
1216  * V4L2 driver and initializes fields of each channel objects
1217  */
1218 static __init int vpif_probe(struct platform_device *pdev)
1219 {
1220 	struct vpif_subdev_info *subdevdata;
1221 	struct i2c_adapter *i2c_adap;
1222 	int subdev_count;
1223 	int res_idx = 0;
1224 	int i, err;
1225 
1226 	if (!pdev->dev.platform_data) {
1227 		dev_warn(&pdev->dev, "Missing platform data.  Giving up.\n");
1228 		return -EINVAL;
1229 	}
1230 
1231 	vpif_dev = &pdev->dev;
1232 	err = initialize_vpif();
1233 
1234 	if (err) {
1235 		v4l2_err(vpif_dev->driver, "Error initializing vpif\n");
1236 		return err;
1237 	}
1238 
1239 	err = v4l2_device_register(vpif_dev, &vpif_obj.v4l2_dev);
1240 	if (err) {
1241 		v4l2_err(vpif_dev->driver, "Error registering v4l2 device\n");
1242 		goto vpif_free;
1243 	}
1244 
1245 	do {
1246 		int irq;
1247 
1248 		err = platform_get_irq_optional(pdev, res_idx);
1249 		if (err < 0 && err != -ENXIO)
1250 			goto vpif_unregister;
1251 		if (err > 0)
1252 			irq = err;
1253 		else
1254 			break;
1255 
1256 		err = devm_request_irq(&pdev->dev, irq, vpif_channel_isr,
1257 				       IRQF_SHARED, VPIF_DRIVER_NAME,
1258 				       (void *)(&vpif_obj.dev[res_idx]->channel_id));
1259 		if (err) {
1260 			vpif_err("VPIF IRQ request failed\n");
1261 			goto vpif_unregister;
1262 		}
1263 	} while (++res_idx);
1264 
1265 	vpif_obj.config = pdev->dev.platform_data;
1266 	subdev_count = vpif_obj.config->subdev_count;
1267 	subdevdata = vpif_obj.config->subdevinfo;
1268 	vpif_obj.sd = kcalloc(subdev_count, sizeof(*vpif_obj.sd), GFP_KERNEL);
1269 	if (!vpif_obj.sd) {
1270 		err = -ENOMEM;
1271 		goto vpif_unregister;
1272 	}
1273 
1274 	i2c_adap = i2c_get_adapter(vpif_obj.config->i2c_adapter_id);
1275 	for (i = 0; i < subdev_count; i++) {
1276 		vpif_obj.sd[i] =
1277 			v4l2_i2c_new_subdev_board(&vpif_obj.v4l2_dev,
1278 						  i2c_adap,
1279 						  &subdevdata[i].board_info,
1280 						  NULL);
1281 		if (!vpif_obj.sd[i]) {
1282 			vpif_err("Error registering v4l2 subdevice\n");
1283 			err = -ENODEV;
1284 			goto probe_subdev_out;
1285 		}
1286 
1287 		vpif_obj.sd[i]->grp_id = 1 << i;
1288 	}
1289 	err = vpif_probe_complete();
1290 	if (err)
1291 		goto probe_subdev_out;
1292 
1293 	return 0;
1294 
1295 probe_subdev_out:
1296 	kfree(vpif_obj.sd);
1297 vpif_unregister:
1298 	v4l2_device_unregister(&vpif_obj.v4l2_dev);
1299 vpif_free:
1300 	free_vpif_objs();
1301 
1302 	return err;
1303 }
1304 
1305 /*
1306  * vpif_remove: It un-register channels from V4L2 driver
1307  */
1308 static void vpif_remove(struct platform_device *device)
1309 {
1310 	struct channel_obj *ch;
1311 	int i;
1312 
1313 	v4l2_device_unregister(&vpif_obj.v4l2_dev);
1314 
1315 	kfree(vpif_obj.sd);
1316 	/* un-register device */
1317 	for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
1318 		/* Get the pointer to the channel object */
1319 		ch = vpif_obj.dev[i];
1320 		/* Unregister video device */
1321 		video_unregister_device(&ch->video_dev);
1322 	}
1323 	free_vpif_objs();
1324 }
1325 
1326 #ifdef CONFIG_PM_SLEEP
1327 static int vpif_suspend(struct device *dev)
1328 {
1329 	struct common_obj *common;
1330 	struct channel_obj *ch;
1331 	int i;
1332 
1333 	for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
1334 		/* Get the pointer to the channel object */
1335 		ch = vpif_obj.dev[i];
1336 		common = &ch->common[VPIF_VIDEO_INDEX];
1337 
1338 		if (!vb2_start_streaming_called(&common->buffer_queue))
1339 			continue;
1340 
1341 		mutex_lock(&common->lock);
1342 		/* Disable channel */
1343 		if (ch->channel_id == VPIF_CHANNEL2_VIDEO) {
1344 			enable_channel2(0);
1345 			channel2_intr_enable(0);
1346 		}
1347 		if (ch->channel_id == VPIF_CHANNEL3_VIDEO ||
1348 			ycmux_mode == 2) {
1349 			enable_channel3(0);
1350 			channel3_intr_enable(0);
1351 		}
1352 		mutex_unlock(&common->lock);
1353 	}
1354 
1355 	return 0;
1356 }
1357 
1358 static int vpif_resume(struct device *dev)
1359 {
1360 
1361 	struct common_obj *common;
1362 	struct channel_obj *ch;
1363 	int i;
1364 
1365 	for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
1366 		/* Get the pointer to the channel object */
1367 		ch = vpif_obj.dev[i];
1368 		common = &ch->common[VPIF_VIDEO_INDEX];
1369 
1370 		if (!vb2_start_streaming_called(&common->buffer_queue))
1371 			continue;
1372 
1373 		mutex_lock(&common->lock);
1374 		/* Enable channel */
1375 		if (ch->channel_id == VPIF_CHANNEL2_VIDEO) {
1376 			enable_channel2(1);
1377 			channel2_intr_enable(1);
1378 		}
1379 		if (ch->channel_id == VPIF_CHANNEL3_VIDEO ||
1380 				ycmux_mode == 2) {
1381 			enable_channel3(1);
1382 			channel3_intr_enable(1);
1383 		}
1384 		mutex_unlock(&common->lock);
1385 	}
1386 
1387 	return 0;
1388 }
1389 
1390 #endif
1391 
1392 static SIMPLE_DEV_PM_OPS(vpif_pm_ops, vpif_suspend, vpif_resume);
1393 
1394 static __refdata struct platform_driver vpif_driver = {
1395 	.driver	= {
1396 			.name	= VPIF_DRIVER_NAME,
1397 			.pm	= &vpif_pm_ops,
1398 	},
1399 	.probe	= vpif_probe,
1400 	.remove_new = vpif_remove,
1401 };
1402 
1403 module_platform_driver(vpif_driver);
1404