vpif_display.c

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