/*
* linux-3.0.8/drivers/media/video/gt2440_cam.c
*/
#include <linux/bug.h>
#include <linux/clk.h>
#include <linux/delay.h>
#include <linux/device.h>
#include <linux/errno.h>
#include <linux/gpio.h>
#include <linux/i2c.h>
#include <linux/interrupt.h>
#include <linux/io.h>
#include <linux/kernel.h>
#include <linux/list.h>
#include <linux/module.h>
#include <linux/platform_device.h>
#include <linux/pm_runtime.h>
#include <linux/slab.h>
#include <linux/types.h>
#include <linux/ratelimit.h>
#include <linux/videodev2.h>

#include <media/v4l2-ctrls.h>
#include <media/v4l2-ioctl.h>
#include <media/v4l2-event.h>
#include <media/v4l2-dev.h>
#include <media/v4l2-device.h>
#include <media/v4l2-subdev.h>
#include <media/videobuf2-core.h>
#include <media/videobuf2-dma-contig.h>
#include <media/v4l2-mediabus.h>
#include "gt2440_ov9650.h"
#include "gt2440_cam.h"

/***** data structure *****/
#define GT2440_CAM_REGISTER_SENSOR_ON_OPEN 

#ifdef GT2440_CAM_DEBUG
static int cam_debug = 1;
#else
static int cam_debug = 0;
#endif
static char *gt2440_cam_clks[CLK_MAX_NUM] = {
	[CLK_GATE]	= "camif",
	[CLK_CAM]	= "camif-upll",
};
static const struct gt2440_cam_fmt gt2440_cam_fmts[] = {
	{	.name		= "YUV 4:2:2 planar, Y/Cb/Cr",
		.fourcc		= V4L2_PIX_FMT_YUV422P,
		.depth		= 16,
		.ybpp		= 1,
		.color		= IMG_FMT_YCBCR422P,
		.colplanes		= 3,
		.flags		= FMT_FL_GT2440_CODEC,
	}, {
		.name		= "YUV 4:2:0 planar, Y/Cb/Cr",
		.fourcc		= V4L2_PIX_FMT_YUV420,
		.depth		= 12,
		.ybpp		= 1,
		.color		= IMG_FMT_YCBCR420,
		.colplanes		= 3,
		.flags		= FMT_FL_GT2440_CODEC,
	}, {
		.name		= "YVU 4:2:0 planar, Y/Cr/Cb",
		.fourcc		= V4L2_PIX_FMT_YVU420,
		.depth		= 12,
		.ybpp		= 1,
		.color		= IMG_FMT_YCRCB420,
		.colplanes		= 3,
		.flags		= FMT_FL_GT2440_CODEC,
	}, {
		.name		= "RGB565, 16 bpp",
		.fourcc		= V4L2_PIX_FMT_RGB565X,
		.depth		= 16,
		.ybpp		= 2,
		.color		= IMG_FMT_RGB565,
		.colplanes		= 1,
		.flags		= FMT_FL_GT2440_PREVIEW,
	}, {
		.name		= "XRGB8888, 32 bpp",
		.fourcc		= V4L2_PIX_FMT_RGB32,
		.depth		= 32,
		.ybpp		= 4,
		.color		= IMG_FMT_XRGB8888,
		.colplanes		= 1,
		.flags		= FMT_FL_GT2440_PREVIEW,
	}
};
static const u32 gt2440_cam_src_pixfmt_map[8][2] = {
	{ V4L2_MBUS_FMT_YUYV8_2X8, CISRCFMT_ORDER422_YCBYCR },
	{ V4L2_MBUS_FMT_YVYU8_2X8, CISRCFMT_ORDER422_YCRYCB },
	{ V4L2_MBUS_FMT_UYVY8_2X8, CISRCFMT_ORDER422_CBYCRY },
	{ V4L2_MBUS_FMT_VYUY8_2X8, CISRCFMT_ORDER422_CRYCBY },
};
static const enum v4l2_mbus_pixelcode gt2440_cam_v4l2_mbus_pixelcodes[] = {
	V4L2_MBUS_FMT_YUYV8_2X8, /* ov9650 used */
	V4L2_MBUS_FMT_YVYU8_2X8,
	V4L2_MBUS_FMT_UYVY8_2X8,
	V4L2_MBUS_FMT_VYUY8_2X8,
};
static const struct gt2440_cam_variant gt2440_cam_var= {
	.vp_pix_limits = {
		[VP_CODEC] = {
			.max_out_width		= 4096,
			.max_sc_out_width	= 2048,
			.out_width_align	= 16,
			.min_out_width		= 16,
			.max_height		= 4096,
		},
		[VP_PREVIEW] = {
			.max_out_width		= 640,
			.max_sc_out_width	= 640,
			.out_width_align	= 16,
			.min_out_width		= 16,
			.max_height		= 480,
		}
	},
	.pix_limits = {
		.win_hor_offset_align	= 8,
	},
};

/***** vb2_ops data structure *****/
static const struct vb2_ops gt2440_cam_vb2_ops = {
	.queue_setup	 = gt2440_cam_queue_setup,
	.wait_prepare	 = gt2440_cam_wait_prepare,
	.wait_finish	 = gt2440_cam_wait_finish,
	.buf_prepare	 = gt2440_cam_buf_prepare,
	.start_streaming = gt2440_cam_start_streaming,
	.stop_streaming	 = gt2440_cam_stop_streaming,
	.buf_queue	 = gt2440_cam_buf_queue,
};
/* v4l2_ctrl_ops */
static const struct v4l2_ctrl_ops gt2440_cam_ctrl_ops = {
	.s_ctrl = gt2440_cam_s_ctrl,
};
/***** v4l2_ioctl_ops data structure *****/
static const struct v4l2_ioctl_ops gt2440_cam_ioctl_ops = {
	.vidioc_querycap		= gt2440_cam_vidioc_querycap,
	.vidioc_enum_fmt_vid_cap = gt2440_cam_vidioc_enum_fmt_vid_cap,

	.vidioc_g_fmt_vid_cap	= gt2440_cam_vidioc_g_fmt_vid_cap,
	.vidioc_s_fmt_vid_cap	= gt2440_cam_vidioc_s_fmt_vid_cap,
	.vidioc_try_fmt_vid_cap	= gt2440_cam_vidioc_try_fmt_vid_cap,

	.vidioc_reqbufs		= gt2440_cam_vidioc_reqbufs,
	.vidioc_querybuf	= gt2440_cam_vidioc_querybuf,
	.vidioc_qbuf		= gt2440_cam_vidioc_qbuf,
	.vidioc_dqbuf		= gt2440_cam_vidioc_dqbuf,
	.vidioc_streamon	= gt2440_cam_vidioc_streamon,
	.vidioc_streamoff	= gt2440_cam_vidioc_streamoff,
	
	.vidioc_enum_input	= gt2440_cam_vidioc_enum_input,
	.vidioc_g_input	= gt2440_cam_vidioc_g_input,
	.vidioc_s_input		= gt2440_cam_vidioc_s_input,
};
/***** v4l2_file_operations data structure *****/
static const struct v4l2_file_operations gt2440_cam_fops = {
	.owner		= THIS_MODULE,
	.open		= gt2440_cam_open,
	.release	= gt2440_cam_release,
	.poll		= gt2440_cam_poll,
	.unlocked_ioctl	= video_ioctl2,
	.mmap		= gt2440_cam_mmap,
};

/***** hardware help function *****/
static void gt2440_cam_hw_dump_regs(struct gt2440_cam_info *info, const char *label)
{
	struct {
		u32 offset;
		const char * const name;
	} registers[] = {
		{ GT2440_CAM_REG_CISRCFMT,		"CISRCFMT" },
		{ GT2440_CAM_REG_CIWDOFST,		"CIWDOFST" },
		{ GT2440_CAM_REG_CIGCTRL,		"CIGCTRL" },
		{ GT2440_CAM_REG_CIWDOFST2,		"CIWDOFST2" },
		
		{ GT2440_CAM_REG_CIYSA(0, 0),		"CICOYSA1" },
		{ GT2440_CAM_REG_CICBSA(0, 0),		"CICOCBSA1" },
		{ GT2440_CAM_REG_CICRSA(0, 0),		"CICOCRSA1" },
		{ GT2440_CAM_REG_CIYSA(0, 1),		"CICOYSA2" },
		{ GT2440_CAM_REG_CICBSA(0, 1),		"CICOCBSA2" },
		{ GT2440_CAM_REG_CICRSA(0, 1),		"CICOCRSA2" },
		{ GT2440_CAM_REG_CIYSA(0, 2),		"CICOYSA3" },
		{ GT2440_CAM_REG_CICBSA(0, 2),		"CICOCBSA3" },
		{ GT2440_CAM_REG_CICRSA(0, 2),		"CICOCRSA3" },
		{ GT2440_CAM_REG_CIYSA(0, 3),		"CICOYSA4" },
		{ GT2440_CAM_REG_CICBSA(0, 3),		"CICOCBSA4" },
		{ GT2440_CAM_REG_CICRSA(0, 3),		"CICOCRSA4" },
		{ GT2440_CAM_REG_CITRGFMT(0, 0),	"CICOTRGFMT" },
		{ GT2440_CAM_REG_CICTRL(0, 0),		"CICOCTRL" },
		{ GT2440_CAM_REG_CISCPRERATIO(0, 0),	"CICOSCPRERATIO" },
		{ GT2440_CAM_REG_CISCPREDST(0, 0),	"CICOSCPREDST" },
		{ GT2440_CAM_REG_CISCCTRL(0, 0),	"CICOSCCTRL" },
		{ GT2440_CAM_REG_CITAREA(0, 0),	"CICOTAREA" },
		{ GT2440_CAM_REG_CISTATUS(0, 0),	"CICOSTATUS" },

		{ GT2440_CAM_REG_CIYSA(1, 0),		"CIPRYSA1" },
		{ GT2440_CAM_REG_CIYSA(1, 1),		"CIPRYSA2" },
		{ GT2440_CAM_REG_CIYSA(1, 2),		"CIPRYSA3" },
		{ GT2440_CAM_REG_CIYSA(1, 3),		"CIPRYSA4" },
		{ GT2440_CAM_REG_CITRGFMT(1, 0),	"CIPRTRGFMT" },
		{ GT2440_CAM_REG_CICTRL(1, 0),		"CIPRCTRL" },
		{ GT2440_CAM_REG_CISCPREDST(1, 0),	"CIPRSCPREDST" },
		{ GT2440_CAM_REG_CISCPRERATIO(1, 0),	"CIPRSCPRERATIO" },
		{ GT2440_CAM_REG_CISCCTRL(1, 0),	"CIPRSCCTRL" },
		{ GT2440_CAM_REG_CITAREA(1, 0),	"CIPRTAREA" },
		{ GT2440_CAM_REG_CISTATUS(1, 0),	"CIPRSTATUS" },
		{ GT2440_CAM_REG_CIIMGCPT(0),		"CIIMGCPT" },
	};
	u32 i;
	pr_info("--- %s ---\n", label);
	for (i = 0; i < ARRAY_SIZE(registers); i++) {
		u32 cfg = readl(info->regs + registers[i].offset);
		printk(KERN_INFO "%s:\t0x%08x\n", registers[i].name, cfg);
	}
}
static void gt2440_cam_hw_reset(struct gt2440_cam_info *info)
{
	u32 cfg;
	cfg = gt2440_cam_hw_read_reg(info, GT2440_CAM_REG_CISRCFMT);
	cfg |= CISRCFMT_ITU601_8BIT;
	gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CISRCFMT, cfg);
	cfg = gt2440_cam_hw_read_reg(info, GT2440_CAM_REG_CIGCTRL);
	cfg |= CIGCTRL_SWRST;
	gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CIGCTRL, cfg);
	udelay(10);
	cfg = gt2440_cam_hw_read_reg(info, GT2440_CAM_REG_CIGCTRL);
	cfg &= ~CIGCTRL_SWRST;
	gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CIGCTRL, cfg);
	udelay(10);
}
static void gt2440_cam_hw_clear_fifo_overflow(struct gt2440_cam_vp *vp)
{
	struct gt2440_cam_info *info = vp->info;
	u32 cfg;
	cfg = gt2440_cam_hw_read_reg(info, GT2440_CAM_REG_CIWDOFST);
	if (vp->id == 0)
		cfg |= (CIWDOFST_CLROVCOFIY | CIWDOFST_CLROVCOFICB |
			CIWDOFST_CLROVCOFICR);
	else
		cfg |= (/* CIWDOFST_CLROVPRFIY | */ CIWDOFST_CLROVPRFICB |
			CIWDOFST_CLROVPRFICR);
	gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CIWDOFST, cfg);
}
static void gt2440_cam_hw_set_camera_bus(struct gt2440_cam_info *info)
{
	unsigned int flags = info->pdata->flags;
	u32 cfg = gt2440_cam_hw_read_reg(info, GT2440_CAM_REG_CIGCTRL);
	cfg &= ~(CIGCTRL_INVPOLPCLK | CIGCTRL_INVPOLVSYNC |
		 CIGCTRL_INVPOLHREF | CIGCTRL_INVPOLFIELD);
	if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
		cfg |= CIGCTRL_INVPOLPCLK;
	if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
		cfg |= CIGCTRL_INVPOLVSYNC;
	if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
		cfg |= CIGCTRL_INVPOLHREF; /* HREF active low */
	gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CIGCTRL, cfg);
}
static void gt2440_cam_hw_set_source_format(struct gt2440_cam_info *info)
{
	struct v4l2_mbus_framefmt *mbus_framefmt = &info->mbus_framefmt;
	unsigned int i = ARRAY_SIZE(gt2440_cam_src_pixfmt_map);
	u32 cfg;
	while (i-- >= 0) {
		if (gt2440_cam_src_pixfmt_map[i][0] == mbus_framefmt->code)
			break;
	}
	if (i == 0 && gt2440_cam_src_pixfmt_map[i][0] != mbus_framefmt->code) {
		dev_err(info->dev,
			"Unsupported pixel code, falling back to %#08x\n",
			gt2440_cam_src_pixfmt_map[i][0]);
	}
	cfg = gt2440_cam_hw_read_reg(info, GT2440_CAM_REG_CISRCFMT);
	cfg &= ~(CISRCFMT_ORDER422_MASK | CISRCFMT_SIZE_CAM_MASK);
	cfg |= (mbus_framefmt->width << 16) | mbus_framefmt->height;
	cfg |= gt2440_cam_src_pixfmt_map[i][1];
	gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CISRCFMT, cfg);
}
static void gt2440_cam_hw_set_camera_rect(struct gt2440_cam_info *info)
{
	struct v4l2_mbus_framefmt *mbus_framefmt = &info->mbus_framefmt;
	struct v4l2_rect *rect = &info->rect;
	u32 cfg;
	cfg = gt2440_cam_hw_read_reg(info, GT2440_CAM_REG_CIWDOFST);
	cfg &= ~(CIWDOFST_OFST_MASK | CIWDOFST_WINOFSEN);
	cfg |= (rect->left << 16) | rect->top;
	if (rect->left != 0 || rect->top != 0)
		cfg |= CIWDOFST_WINOFSEN;
	gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CIWDOFST, cfg);
}
static void gt2440_cam_hw_set_test_pattern(struct gt2440_cam_info *info, 
									unsigned int pattern)
{
	u32 cfg = gt2440_cam_hw_read_reg(info, GT2440_CAM_REG_CIGCTRL);
	cfg &= ~CIGCTRL_TESTPATTERN_MASK;
	cfg |= (pattern << 27);
	gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CIGCTRL, cfg);
}
static void gt2440_cam_hw_set_effect(struct gt2440_cam_info *info, unsigned int effect,
								unsigned int cr, unsigned int cb)
{
}
static int gt2440_cam_get_scaler_factor(u32 src, u32 tar, u32 *ratio, u32 *shift)
{
	unsigned int sh = 6;
	if (src >= 64 * tar)
		return -EINVAL;
	while (sh--) {
		unsigned int tmp = 1 << sh;
		if (src >= tar * tmp) {
			*shift = sh, *ratio = tmp;
			return 0;
		}
	}
	*shift = 0, *ratio = 1;
	return 0;
}
static int gt2440_cam_get_scaler_config(struct gt2440_cam_vp *vp,
				struct gt2440_cam_scaler *scaler)
{
	struct v4l2_rect *rect = &vp->info->rect;
	int source_x = rect->width;
	int source_y = rect->height;
	int target_x = vp->out_frame.rect.width;
	int target_y = vp->out_frame.rect.height;
	int ret;
	if (vp->rotation == 90 || vp->rotation == 270)
		swap(target_x, target_y);
	ret = gt2440_cam_get_scaler_factor(source_x, target_x, &scaler->pre_h_ratio,
				      &scaler->h_shift);
	if (ret < 0)
		return ret;
	ret = gt2440_cam_get_scaler_factor(source_y, target_y, &scaler->pre_v_ratio,
				      &scaler->v_shift);
	if (ret < 0)
		return ret;
	scaler->pre_dst_width = source_x / scaler->pre_h_ratio;
	scaler->pre_dst_height = source_y / scaler->pre_v_ratio;
	scaler->main_h_ratio = (source_x << 8) / (target_x << scaler->h_shift);
	scaler->main_v_ratio = (source_y << 8) / (target_y << scaler->v_shift);
	scaler->scaleup_h = (target_x >= source_x);
	scaler->scaleup_v = (target_y >= source_y);
	scaler->copy = 0;
	return 0;
}
static void gt2440_cam_hw_set_prescaler(struct gt2440_cam_vp *vp)
{
	struct gt2440_cam_info *info= vp->info;
	struct gt2440_cam_scaler *scaler= &vp->scaler;
	u32 cfg, shfactor, addr;
	addr = GT2440_CAM_REG_CISCPRERATIO(vp->id, vp->offset);
	shfactor = 10 - (scaler->h_shift + scaler->v_shift);
	cfg = shfactor << 28;
	cfg |= (scaler->pre_h_ratio << 16) | scaler->pre_v_ratio;
	gt2440_cam_hw_write_reg(info, addr, cfg);
	cfg = (scaler->pre_dst_width << 16) | scaler->pre_dst_height;
	gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CISCPREDST(vp->id, vp->offset), cfg);
}
static void gt2440_cam_hw_set_scaler(struct gt2440_cam_vp *vp)
{
	struct gt2440_cam_info *info= vp->info;
	struct gt2440_cam_scaler *scaler = &vp->scaler;
	unsigned int color = vp->out_fmt->color;
	u32 cfg;
	gt2440_cam_hw_set_prescaler(vp);
	cfg = gt2440_cam_hw_read_reg(info, GT2440_CAM_REG_CISCCTRL(vp->id, vp->offset));
	cfg &= ~(CISCCTRL_SCALEUP_MASK | CISCCTRL_SCALERBYPASS |
		 CISCCTRL_MAIN_RATIO_MASK | CIPRSCCTRL_RGB_FORMAT_24BIT);
	if (scaler->enable) {
		if (scaler->scaleup_h) {
			if (vp->id == VP_CODEC)
				cfg |= CISCCTRL_SCALEUP_H;
			else
				cfg |= CIPRSCCTRL_SCALEUP_H;
		}
		if (scaler->scaleup_v) {
			if (vp->id == VP_CODEC)
				cfg |= CISCCTRL_SCALEUP_V;
			else
				cfg |= CIPRSCCTRL_SCALEUP_V;
		}
	} else {
		if (vp->id == VP_CODEC)
			cfg |= CISCCTRL_SCALERBYPASS;
	}
	cfg |= ((scaler->main_h_ratio & 0x1ff) << 16);
	cfg |= scaler->main_v_ratio & 0x1ff;
	if (vp->id == VP_PREVIEW) {
		if (color == IMG_FMT_XRGB8888)
			cfg |= CIPRSCCTRL_RGB_FORMAT_24BIT;
		cfg |= CIPRSCCTRL_SAMPLE;
	}
	gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CISCCTRL(vp->id, vp->offset), cfg);
}
static void gt2440_cam_hw_enable_scaler(struct gt2440_cam_vp *vp, bool on)
{
	u32 addr = GT2440_CAM_REG_CISCCTRL(vp->id, vp->offset);
	u32 cfg;
	cfg = gt2440_cam_hw_read_reg(vp->info, addr);
	if (on)
		cfg |= CISCCTRL_SCALERSTART;
	else
		cfg &= ~CISCCTRL_SCALERSTART;
	gt2440_cam_hw_write_reg(vp->info, addr, cfg);
}
static void gt2440_cam_hw_set_flip(struct gt2440_cam_vp *vp)
{
	u32 cfg = gt2440_cam_hw_read_reg(vp->info,
				GT2440_CAM_REG_CITRGFMT(vp->id, vp->offset));
	cfg &= ~CITRGFMT_FLIP_MASK;
	if (vp->hflip)
		cfg |= CITRGFMT_FLIP_Y_MIRROR;
	if (vp->vflip)
		cfg |= CITRGFMT_FLIP_X_MIRROR;
	gt2440_cam_hw_write_reg(vp->info, 
		GT2440_CAM_REG_CITRGFMT(vp->id, vp->offset), cfg);
}
static void gt2440_cam_hw_set_target_format(struct gt2440_cam_vp *vp)
{
	struct gt2440_cam_info *info = vp->info;
	struct gt2440_cam_frame *frame = &vp->out_frame;
	u32 cfg;
	cfg = gt2440_cam_hw_read_reg(info, GT2440_CAM_REG_CITRGFMT(vp->id, vp->offset));
	cfg &= ~CITRGFMT_TARGETSIZE_MASK;
	cfg |= CITRGFMT_IN422;
	cfg &= ~CITRGFMT_OUT422;
	if (vp->out_fmt->color == IMG_FMT_YCBCR422P)
		cfg |= CITRGFMT_OUT422;
	if (vp->rotation == 90 || vp->rotation == 270)
		cfg |= (frame->f_height << 16) | frame->f_width;
	else
		cfg |= (frame->f_width << 16) | frame->f_height;
	gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CITRGFMT(vp->id, vp->offset), cfg);
	cfg = gt2440_cam_hw_read_reg(info, GT2440_CAM_REG_CITAREA(vp->id, vp->offset));
	cfg &= ~CITAREA_MASK;
	cfg |= (frame->f_width * frame->f_height);
	gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CITAREA(vp->id, vp->offset), cfg);
}
static void gt2440_cam_hw_set_out_dma_size(struct gt2440_cam_vp *vp)
{
	struct gt2440_cam_frame *frame = &vp->out_frame;
	u32 cfg;
	cfg = gt2440_cam_hw_read_reg(vp->info, GT2440_CAM_REG_CITRGFMT(vp->id, vp->offset));
	cfg &= ~CITRGFMT_TARGETSIZE_MASK;
	cfg |= (frame->f_width << 16) | frame->f_height;
	gt2440_cam_hw_write_reg(vp->info, GT2440_CAM_REG_CITRGFMT(vp->id, vp->offset), cfg);
}
static void gt2440_cam_get_dma_burst(u32 width, u32 ybpp, u32 *mburst, u32 *rburst)
{
	unsigned int nwords = width * ybpp / 4;
	unsigned int div, rem;
	if (WARN_ON(width < 8 || (width * ybpp) & 7))
		return;
	for (div = 16; div >= 2; div /= 2) {
		if (nwords < div)
			continue;
		rem = nwords & (div - 1);
		if (rem == 0) {
			*mburst = div;
			*rburst = div;
			break;
		}
		if (rem == div / 2 || rem == div / 4) {
			*mburst = div;
			*rburst = rem;
			break;
		}
	}
}
void gt2440_cam_hw_set_output_dma(struct gt2440_cam_vp *vp)
{
	struct gt2440_cam_info *info = vp->info;
	struct gt2440_cam_frame *frame = &vp->out_frame;
	const struct gt2440_cam_fmt *fmt = vp->out_fmt;
	unsigned int ymburst = 0, yrburst = 0;
	u32 cfg;
	gt2440_cam_hw_set_out_dma_size(vp);
	gt2440_cam_get_dma_burst(frame->rect.width, fmt->ybpp, &ymburst, &yrburst);
	cfg = gt2440_cam_hw_read_reg(info, GT2440_CAM_REG_CICTRL(vp->id, vp->offset));
	cfg &= ~CICTRL_BURST_MASK;
	cfg |= CICTRL_YBURST1(ymburst) | CICTRL_YBURST2(yrburst);
	cfg |= CICTRL_CBURST1(ymburst / 2) | CICTRL_CBURST2(yrburst / 2);
	gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CICTRL(vp->id, vp->offset), cfg);
}
static void gt2440_cam_hw_cfg_path(struct gt2440_cam_vp *vp)
{
	WARN_ON(gt2440_cam_get_scaler_config(vp, &vp->scaler));
	gt2440_cam_hw_set_scaler(vp);
	gt2440_cam_hw_set_flip(vp);
	gt2440_cam_hw_set_target_format(vp);
	gt2440_cam_hw_set_output_dma(vp);
}
static int gt2440_cam_hw_init(struct gt2440_cam_info *info, 
							struct gt2440_cam_vp *vp)
{
	const struct gt2440_cam_variant *variant = info->variant;
	if (info->sensor.v4l2_sub == NULL || vp->out_fmt == NULL)
		return -EINVAL;
	gt2440_cam_hw_clear_fifo_overflow(vp);
	gt2440_cam_hw_set_camera_bus(info);
	gt2440_cam_hw_set_source_format(info);
	gt2440_cam_hw_set_camera_rect(info);
	gt2440_cam_hw_set_test_pattern(info, info->test_pattern);
	if (variant->has_img_effect)
		gt2440_cam_hw_set_effect(info, info->colorfx,
				info->colorfx_cb, info->colorfx_cr);
	gt2440_cam_hw_cfg_path(vp); /* codec or preview */
	vp->state &= ~ST_VP_CONFIG;
	return 0;
}
static void gt2440_cam_hw_prepare_dma_offset(struct gt2440_cam_vp *vp)
{
	struct gt2440_cam_frame *frame = &vp->out_frame;
	frame->dma_offset.initial = frame->rect.top * frame->f_width + frame->rect.left;
	frame->dma_offset.line = frame->f_width - (frame->rect.left + frame->rect.width);
}
static int gt2440_cam_hw_vp_init(struct gt2440_cam_info *info, 
							struct gt2440_cam_vp *vp)
{
	if (vp->out_fmt == NULL)
		return -EINVAL;
	gt2440_cam_hw_prepare_dma_offset(vp);
	gt2440_cam_hw_clear_fifo_overflow(vp);
	gt2440_cam_hw_cfg_path(vp);
	vp->state &= ~ST_VP_CONFIG;
	return 0;
}

static void gt2440_cam_hw_enable_capture(struct gt2440_cam_vp *vp)
{
	struct gt2440_cam_info *info = vp->info;
	u32 cfg;
	cfg = gt2440_cam_hw_read_reg(info, GT2440_CAM_REG_CIIMGCPT(vp->offset));
	info->stream_count++;
	if (vp->scaler.enable)
		cfg |= CIIMGCPT_IMGCPTEN_SC(vp->id);
	if (info->stream_count == 1)
		cfg |= CIIMGCPT_IMGCPTEN;
	gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CIIMGCPT(vp->offset), cfg);
}
static void gt2440_cam_hw_disable_capture(struct gt2440_cam_vp *vp)
{
	struct gt2440_cam_info *info = vp->info;
	u32 cfg;

	cfg = gt2440_cam_hw_read_reg(info, GT2440_CAM_REG_CIIMGCPT(vp->offset));
	cfg &= ~ CIIMGCPT_IMGCPTEN_SC(vp->id);
	if (WARN_ON(--(info->stream_count) < 0))
		info->stream_count = 0;
	if (info->stream_count == 0)
		cfg &= ~CIIMGCPT_IMGCPTEN;
	gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CIIMGCPT(vp->offset), cfg);
}
void gt2440_cam_hw_set_output_addr(struct gt2440_cam_vp *vp,
			      struct gt2440_cam_addr *paddr, int i)
{
	struct gt2440_cam_info *info = vp->info;
	gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CIYSA(vp->id, i), paddr->y);
	if (vp->id == VP_CODEC) {
		gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CICBSA(vp->id, i),
								paddr->cb);
		gt2440_cam_hw_write_reg(info, GT2440_CAM_REG_CICRSA(vp->id, i),
								paddr->cr);
	}
}



/***** vb2_ops help function *****/
static int gt2440_cam_prepare_addr(struct gt2440_cam_vp *vp, 
							struct vb2_buffer *vb2_b,
			      				struct gt2440_cam_addr *paddr)
{
	struct gt2440_cam_frame *frame = &vp->out_frame;
	u32 pix_size;
	if (vb2_b == NULL || frame == NULL)
		return -EINVAL;
	pix_size = frame->rect.width * frame->rect.height;
	paddr->y = vb2_dma_contig_plane_paddr(vb2_b, 0);
	switch (vp->out_fmt->colplanes) {
	case 1:
		paddr->cb = 0;
		paddr->cr = 0;
		break;
	case 2:
		paddr->cb = (u32)(paddr->y + pix_size);
		paddr->cr = 0;
		break;
	case 3:
		paddr->cb = (u32)(paddr->y + pix_size);
		if (vp->out_fmt->color == IMG_FMT_YCBCR422P)
			paddr->cr = (u32)(paddr->cb + (pix_size >> 1));
		else 
			paddr->cr = (u32)(paddr->cb + (pix_size >> 2));
		if (vp->out_fmt->color == IMG_FMT_YCRCB420)
			swap(paddr->cb, paddr->cr);
		break;
	default:
		return -EINVAL;
	}
	return 0;
}
static int gt2440_cam_sensor_set_power(struct gt2440_cam_info *info, int on)
{
	struct gt2440_cam_sensor *sensor = &info->sensor;
	int ret = 0;
	if (!on == sensor->power_count)
		ret = v4l2_subdev_call(sensor->v4l2_sub, core, s_power, on);
	if (!ret)
		sensor->power_count += on ? 1 : -1;
	return ret;
}
static int gt2440_cam_sensor_set_streaming(struct gt2440_cam_info *info, int on)
{	dprintk("gt2440_cam_sensor_set_streaming(on=%d)\n", on);
	struct gt2440_cam_sensor *sensor = &info->sensor;
	printk(KERN_INFO"sensor =0x%08x", sensor);
	printk(KERN_INFO"sensor->v4l2_sub addr=0x%08x", sensor->v4l2_sub);
	int ret = 0;
	if (!on == sensor->stream_count)
		ret = v4l2_subdev_call(sensor->v4l2_sub, video, s_stream, on);
	if (!ret)
		sensor->stream_count += on ? 1 : -1;
	return ret;
}

static int gt2440_cam_reinitialize(struct gt2440_cam_vp *vp)
{
	struct gt2440_cam_info *info = vp->info;
	struct gt2440_cam_buffer *buf;
	unsigned long flags;
	bool streaming;
	spin_lock_irqsave(&info->lock, flags);
	streaming = vp->state & ST_VP_SENSOR_STREAMING;
	vp->state &= ~(ST_VP_PENDING | ST_VP_RUNNING | ST_VP_OFF |
		       ST_VP_ABORTING | ST_VP_STREAMING |
		       ST_VP_SENSOR_STREAMING | ST_VP_LASTIRQ);
	while (!list_empty(&vp->pending_buf_q)) {
		buf = gt2440_cam_pending_queue_pop(vp);
		vb2_buffer_done(&buf->vb2_b, VB2_BUF_STATE_ERROR);
	}
	while (!list_empty(&vp->active_buf_q)) {
		buf = gt2440_cam_active_queue_pop(vp);
		vb2_buffer_done(&buf->vb2_b, VB2_BUF_STATE_ERROR);
	}
	spin_unlock_irqrestore(&info->lock, flags);
	if (!streaming)
		return 0;
	return gt2440_cam_sensor_set_streaming(info, 0);
}
static bool gt2440_cam_vp_active(struct gt2440_cam_vp *vp)
{
	struct gt2440_cam_info *info = vp->info;
	unsigned long flags;
	bool ret;
	spin_lock_irqsave(&info->lock, flags);
	ret = (vp->state & ST_VP_RUNNING) || (vp->state & ST_VP_PENDING);
	spin_unlock_irqrestore(&info->lock, flags);
	return ret;
}
static int gt2440_cam_stop_capture(struct gt2440_cam_vp *vp)
{
	struct gt2440_cam_info *info = vp->info;
	unsigned long flags;
	int ret;
	if (!gt2440_cam_vp_active(vp))
		return 0;
	spin_lock_irqsave(&info->lock, flags);
	vp->state &= ~(ST_VP_OFF | ST_VP_LASTIRQ);
	vp->state |= ST_VP_ABORTING;
	spin_unlock_irqrestore(&info->lock, flags);
	ret = wait_event_timeout(vp->irq_queue,
			   !(vp->state & ST_VP_ABORTING),
			   msecs_to_jiffies(CAM_STOP_TIMEOUT));
	spin_lock_irqsave(&info->lock, flags);
	if (ret == 0 && !(vp->state & ST_VP_OFF)) {
		vp->state &= ~(ST_VP_OFF | ST_VP_ABORTING |
			       ST_VP_LASTIRQ);
		gt2440_cam_hw_disable_capture(vp);
		gt2440_cam_hw_enable_scaler(vp, false);
	}
	spin_unlock_irqrestore(&info->lock, flags);
	return gt2440_cam_reinitialize(vp);
}



/***** vb2_ops function *****/
static int gt2440_cam_start_streaming(struct vb2_queue *vb2_q)
{	dprintk("gt2440_cam_start_streaming()\n");
	struct gt2440_cam_vp *vp = vb2_get_drv_priv(vb2_q);
	struct gt2440_cam_info *info = vp->info;
	unsigned long flags;
	int ret;
	spin_lock_irqsave(&info->lock, flags);
	if (info->stream_count == 0) {
		gt2440_cam_hw_reset(info);
		ret = gt2440_cam_hw_init(info, vp);
	} else {
		ret = gt2440_cam_hw_vp_init(info, vp);
	}
	spin_unlock_irqrestore(&info->lock, flags);
	if (ret < 0) {
		dev_err(info->dev, "failed to init hw: error =%d", ret);
		gt2440_cam_reinitialize(vp);
		return ret;
	}
	spin_lock_irqsave(&info->lock, flags);
	vp->frame_sequence = 0;
	vp->state |= ST_VP_PENDING;
	if (!list_empty(&vp->pending_buf_q) &&
	    (!(vp->state & ST_VP_STREAMING) ||  
	    	!(vp->state & ST_VP_SENSOR_STREAMING))) {
		gt2440_cam_hw_enable_scaler(vp, vp->scaler.enable);
		gt2440_cam_hw_enable_capture(vp);
		vp->state |= ST_VP_STREAMING;
		if (!(vp->state & ST_VP_SENSOR_STREAMING)) {
			vp->state |= ST_VP_SENSOR_STREAMING;
			spin_unlock_irqrestore(&info->lock, flags);
			ret = gt2440_cam_sensor_set_streaming(info, 1);
			if (ret)
				dev_err(vp->info->dev, "failed to set Sensor s_stream(tart_streaming) \n");
			if (cam_debug)
				gt2440_cam_hw_dump_regs(info, __func__);
			return ret;
		}
	}
	spin_unlock_irqrestore(&info->lock, flags);
	return 0;
}
static int gt2440_cam_stop_streaming(struct vb2_queue *vb2_q)
{	dprintk("gt2440_cam_stop_streaming()\n");
	struct gt2440_cam_vp *vp = vb2_get_drv_priv(vb2_q);
	//return gt2440_cam_stop_capture(vb2_q); /* the bug is on here, my god! */
	return gt2440_cam_stop_capture(vp);
}
static void gt2440_cam_buf_queue(struct vb2_buffer *vb2_b)
{	//dprintk("gt2440_cam_buf_queue()\n");
	struct gt2440_cam_buffer *buf = container_of(vb2_b, struct gt2440_cam_buffer, vb2_b);
	struct gt2440_cam_vp *vp = vb2_get_drv_priv(vb2_b->vb2_queue);
	struct gt2440_cam_info *info = vp->info;
	unsigned long flags;
	spin_lock_irqsave(&info->lock, flags);
	WARN_ON(gt2440_cam_prepare_addr(vp, &buf->vb2_b, &buf->paddr));
	if (!(vp->state & ST_VP_STREAMING) && vp->active_buffers < 2) {
		buf->index = vp->buf_index;
		gt2440_cam_hw_set_output_addr(vp, &buf->paddr, buf->index);
		gt2440_cam_hw_set_output_addr(vp, &buf->paddr, buf->index + 2);
		gt2440_cam_active_queue_add(vp, buf);
		vp->buf_index = !vp->buf_index;
	} else {
		gt2440_cam_pending_queue_add(vp, buf);
	}
	if (vb2_is_streaming(&vp->vb2_q) && 
		!list_empty(&vp->pending_buf_q) && 
		!(vp->state & ST_VP_STREAMING)) {
		
		vp->state |= ST_VP_STREAMING;
		gt2440_cam_hw_enable_scaler(vp, vp->scaler.enable);
		gt2440_cam_hw_enable_capture(vp);
		spin_unlock_irqrestore(&info->lock, flags);
		if (!(vp->state & ST_VP_SENSOR_STREAMING)) {
			if (gt2440_cam_sensor_set_streaming(info, 1) == 0)
				vp->state |= ST_VP_SENSOR_STREAMING;
			else
				dev_err(vp->info->dev, "failed to set Sensor s_stream(buf_queue) \n");
			if (cam_debug){
				gt2440_cam_hw_dump_regs(info, __func__);
			}
		}
		return;
	}
	spin_unlock_irqrestore(&info->lock, flags);
}
static int gt2440_cam_queue_setup(struct vb2_queue *vb2_q, 
			unsigned int *num_buffers, unsigned int *num_planes, 
			unsigned long sizes[],  void *allocators[])
{	//dprintk("gt2440_cam_queue_setup()\n");
	const struct v4l2_pix_format *pix_format = NULL;
	struct gt2440_cam_vp *vp = vb2_get_drv_priv(vb2_q);
	struct gt2440_cam_info *info = vp->info;
	struct gt2440_cam_frame *frame = &vp->out_frame;
	const struct gt2440_cam_fmt *fmt = vp->out_fmt;
	unsigned int size;
	size = (frame->f_width * frame->f_height * fmt->depth) / 8;
	if (fmt == NULL)
		return -EINVAL;
	*num_planes = 1;
	if (pix_format)
		sizes[0] = max(size, pix_format->sizeimage);
	else
		sizes[0] = size;
	allocators[0] = info->alloc_ctx;
	return 0;
}
static void gt2440_cam_wait_prepare(struct vb2_queue *vb2_q)
{	//dprintk("gt2440_cam_wait_prepare()\n");
	struct gt2440_cam_vp *vp = vb2_get_drv_priv(vb2_q);
	mutex_unlock(&vp->info->mutex);
}
static void gt2440_cam_wait_finish(struct vb2_queue *vb2_q)
{	//dprintk("gt2440_cam_wait_finish()\n");
	struct gt2440_cam_vp *vp = vb2_get_drv_priv(vb2_q);
	mutex_lock(&vp->info->mutex);
}
static int gt2440_cam_buf_prepare(struct vb2_buffer *vb2_b)
{	//dprintk("gt2440_cam_buf_prepare()\n");
	struct gt2440_cam_vp *vp = vb2_get_drv_priv(vb2_b->vb2_queue);
	if (vp->out_fmt == NULL)
		return -EINVAL;
	if (vb2_plane_size(vb2_b, 0) < vp->payload) {
		dev_err(vp->info->dev, "failed to get buffer too small: %lu, required: %u\n",
			 vb2_plane_size(vb2_b, 0), vp->payload);
		return -EINVAL;
	}
	vb2_set_plane_payload(vb2_b, 0, vp->payload);
	return 0;
}



/***** v4l2_ctrl_ops function *****/
static int gt2440_cam_s_ctrl(struct v4l2_ctrl *ctrl)
{
	struct gt2440_cam_vp *vp = ctrl->priv;
	struct gt2440_cam_info *info = vp->info;
	unsigned long flags;
	spin_lock_irqsave(&info->lock, flags);
	switch (ctrl->id) {
	case V4L2_CID_HFLIP:
		vp->hflip = ctrl->val;
		break;
	case V4L2_CID_VFLIP:
		vp->vflip = ctrl->val;
		break;
	}
	vp->state |= ST_VP_CONFIG;
	spin_unlock_irqrestore(&info->lock, flags);
	return 0;
}

/***** v4l2_ioctl_ops function *****/
const struct gt2440_cam_fmt *gt2440_cam_find_format(struct gt2440_cam_vp *vp,
					      const u32 *pixelformat,
					      int index)
{
	const struct gt2440_cam_fmt *fmt, *def_fmt = NULL;
	unsigned int i;
	int id = 0;
	if (index >= (int)ARRAY_SIZE(gt2440_cam_fmts))
		return NULL;
	for (i = 0; i < ARRAY_SIZE(gt2440_cam_fmts); ++i) {
		fmt = &gt2440_cam_fmts[i];
		if (vp && !(vp->fmt_flags & fmt->flags))
			continue;
		if (pixelformat && fmt->fourcc == *pixelformat)
			return fmt;
		if (index == id)
			def_fmt = fmt;
		id++;
	}
	return def_fmt;
}
static int gt2440_cam_try_fmt(struct gt2440_cam_vp *vp,
				    struct v4l2_pix_format *format,
				    const struct gt2440_cam_fmt **ffmt)
{
	struct gt2440_cam_info *info = vp->info;
	struct v4l2_rect *rect  = &info->rect ;
	unsigned int wmin, hmin, sc_hrmax, sc_vrmax;
	const struct gt2440_cam_vp_pix_limits *vp_pix_limits;
	const struct gt2440_cam_fmt *fmt;
	fmt = gt2440_cam_find_format(vp, &format->pixelformat, 0);
	if (WARN_ON(fmt == NULL))
		return -EINVAL;
	if (ffmt)
		*ffmt = fmt;
	vp_pix_limits = &info->variant->vp_pix_limits[vp->id];
	sc_hrmax = min(SCALER_MAX_RATIO, 1 << (ffs(rect->width) - 3));
	sc_vrmax = min(SCALER_MAX_RATIO, 1 << (ffs(rect->height) - 1));
	wmin = max_t(u32, vp_pix_limits->min_out_width, rect->width / sc_hrmax);
	wmin = round_up(wmin, vp_pix_limits->out_width_align);
	hmin = max_t(u32, 8, rect->height / sc_vrmax);
	hmin = round_up(hmin, 8);
	v4l_bound_align_image(&format->width, wmin, vp_pix_limits->max_sc_out_width,
			      ffs(vp_pix_limits->out_width_align) - 1,
			      &format->height, hmin, vp_pix_limits->max_height, 0, 0);
	format->bytesperline = format->width * fmt->ybpp;
	format->sizeimage = (format->width * format->height * fmt->depth) / 8;
	format->pixelformat = fmt->fourcc;
	format->colorspace = V4L2_COLORSPACE_JPEG;
	format->field = V4L2_FIELD_NONE;
	return 0;
}
static int gt2440_cam_vidioc_querycap(struct file *file, void *priv,
				     struct v4l2_capability *cap)
{	//dprintk("gt2440_cam_vidioc_querycap()\n");
	struct gt2440_cam_vp *vp = video_drvdata(file);
	strlcpy(cap->driver, PLAT_DEVICE_NAME, sizeof(cap->driver));
	strlcpy(cap->card, PLAT_DEVICE_NAME, sizeof(cap->card));
	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s.%d",
		 dev_name(vp->info->dev), vp->id);
	cap->capabilities = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE;
	return 0;
}
static int gt2440_cam_vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
				     struct v4l2_fmtdesc *desc)
{
	struct gt2440_cam_vp *vp = video_drvdata(file);
	const struct gt2440_cam_fmt *fmt;
	fmt = gt2440_cam_find_format(vp, NULL, desc->index);
	if (!fmt)
		return -EINVAL;
	strlcpy(desc->description, fmt->name, sizeof(desc->description));
	desc->pixelformat = fmt->fourcc;
	return 0;
}
static int gt2440_cam_vidioc_g_fmt_vid_cap(struct file *file, void *priv,
				  struct v4l2_format *format)
{
	struct gt2440_cam_vp *vp = video_drvdata(file);
	struct v4l2_pix_format *pix_format = &format->fmt.pix;
	struct gt2440_cam_frame *frame = &vp->out_frame;
	const struct gt2440_cam_fmt *fmt = vp->out_fmt;
	pix_format->bytesperline = frame->f_width * fmt->ybpp;
	pix_format->sizeimage = vp->payload;
	pix_format->pixelformat = fmt->fourcc;
	pix_format->width = frame->f_width;
	pix_format->height = frame->f_height;
	pix_format->field = V4L2_FIELD_NONE;
	pix_format->colorspace = V4L2_COLORSPACE_JPEG;
	return 0;
}
static int gt2440_cam_vidioc_s_fmt_vid_cap(struct file *file, void *priv,
				  struct v4l2_format *format)
{	//dprintk("gt2440_cam_vidioc_s_fmt_vid_cap()\n");
	struct v4l2_pix_format *pix_format = &format->fmt.pix;
	struct gt2440_cam_vp *vp = video_drvdata(file);
	struct gt2440_cam_frame *frame = &vp->out_frame;
	const struct gt2440_cam_fmt *fmt = NULL;
	int ret;
	if (vb2_is_busy(&vp->vb2_q))
		return -EBUSY;
	ret = gt2440_cam_try_fmt(vp, &format->fmt.pix, &fmt);
	if (ret < 0)
		return ret;
	vp->out_fmt = fmt;
	vp->payload = pix_format->sizeimage;
	frame->f_width = pix_format->width;
	frame->f_height = pix_format->height;
	frame->rect.width = pix_format->width;
	frame->rect.height = pix_format->height;
	frame->rect.left = 0;
	frame->rect.top = 0;
	if (vp->owner == NULL)
		vp->owner = priv;
	return 0;
}
static int gt2440_cam_vidioc_try_fmt_vid_cap(struct file *file, void *priv,
				    struct v4l2_format *format)
{
	struct gt2440_cam_vp *vp = video_drvdata(file);
	return gt2440_cam_try_fmt(vp, &format->fmt.pix, NULL);
}
static int gt2440_cam_vidioc_reqbufs(struct file *file, void *priv,
			     struct v4l2_requestbuffers *req)
{	dprintk("gt2440_cam_vidioc_reqbufs()\n");
	struct gt2440_cam_vp *vp = video_drvdata(file);
	int ret;
	if (vp->owner && vp->owner != priv)
		return -EBUSY;
	if (req->count)
		req->count = max_t(u32, CAM_REQ_BUFS_MIN, req->count);
	else
		vp->owner = NULL;
	ret = vb2_reqbufs(&vp->vb2_q, req);
	if (!ret) {
		vp->reqbufs_count = req->count;
		if (vp->owner == NULL && req->count > 0)
			vp->owner = priv;
	}
	return ret;
}
static int gt2440_cam_vidioc_querybuf(struct file *file, void *priv,
			      struct v4l2_buffer *buf)
{	dprintk("gt2440_cam_vidioc_querybuf()\n");
	struct gt2440_cam_vp *vp = video_drvdata(file);
	int ret = 0;
	ret = vb2_querybuf(&vp->vb2_q, buf);
	if(ret < 0){
		dev_err(vp->info->dev, "failed to query buffers: error = %d\n", ret);
		return ret;
	}
	return ret;
}
static int gt2440_cam_vidioc_qbuf(struct file *file, void *priv,
			  struct v4l2_buffer *buf)
{	//dprintk("gt2440_cam_vidioc_qbuf()\n");
	struct gt2440_cam_vp *vp = video_drvdata(file);
	if (vp->owner && vp->owner != priv)
		return -EBUSY;
	int ret = 0;
	ret = vb2_qbuf(&vp->vb2_q, buf);
	if(ret < 0){
		dev_err(vp->info->dev, "failed to queue buffers: error = %d\n", ret);
		return ret;
	}
	return ret;
}
static int gt2440_cam_vidioc_dqbuf(struct file *file, void *priv,
			   struct v4l2_buffer *buf)
{	//dprintk("gt2440_cam_vidioc_dqbuf()\n");
	struct gt2440_cam_vp *vp = video_drvdata(file);
	if (vp->owner && vp->owner != priv)
		return -EBUSY;
	int ret = 0;
	ret = vb2_dqbuf(&vp->vb2_q, buf, file->f_flags & O_NONBLOCK);
	if(ret < 0){
		dev_err(vp->info->dev, "failed to dequeue buffers: error = %d\n", ret);
		return ret;
	}
	return ret;
}
static int gt2440_cam_vidioc_streamon(struct file *file, void *priv,
			      enum v4l2_buf_type type)
{	//dprintk("gt2440_cam_vidioc_streamon()\n");
	struct gt2440_cam_vp *vp = video_drvdata(file);
	struct gt2440_cam_info *info = vp->info;
	int ret;
	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
		return -EINVAL;
	if (vp->owner && vp->owner != priv)
		return -EBUSY;
	if (gt2440_cam_vp_active(vp))
		return 0;
	ret = vb2_streamon(&vp->vb2_q, type);
	if(ret < 0){
		dev_err(vp->info->dev, "failed to stream on: error = %d\n", ret);
		return ret;
	}
	return ret;
}
static int gt2440_cam_vidioc_streamoff(struct file *file, void *priv,
			       enum v4l2_buf_type type)
{	//dprintk("gt2440_cam_vidioc_streamoff()\n");
	struct gt2440_cam_vp *vp = video_drvdata(file);
	struct gt2440_cam_info *info = vp->info;
	int ret;
	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
		return -EINVAL;
	if (vp->owner && vp->owner != priv)
		return -EBUSY;
	ret = vb2_streamoff(&vp->vb2_q, type);
	if(ret < 0){
		dev_err(vp->info->dev, "failed to stream off: error = %d\n", ret);
		return ret;
	}
	return ret;
}
static int gt2440_cam_vidioc_enum_input(struct file *file, void *priv,
				       struct v4l2_input *input)
{
	struct gt2440_cam_vp *vp = video_drvdata(file);
	struct v4l2_subdev *v4l2_sub = vp->info->sensor.v4l2_sub;
	if (input->index || v4l2_sub == NULL)
		return -EINVAL;
	input->type = V4L2_INPUT_TYPE_CAMERA;
	strlcpy(input->name, v4l2_sub->name, sizeof(input->name));
	return 0;
}
static int gt2440_cam_vidioc_g_input(struct file *file, void *priv,
				    unsigned int *i)
{
	*i = 0;	return 0;
}
static int gt2440_cam_vidioc_s_input(struct file *file, void *priv,
				    unsigned int i)
{
	return i == 0 ? 0 : -EINVAL;
}


/***** v4l2_file_operations function *****/
static unsigned int gt2440_cam_poll(struct file *file,
				   struct poll_table_struct *wait)
{	//dprintk("gt2440_cam_poll()\n");
	struct gt2440_cam_vp *vp = video_drvdata(file);
	struct gt2440_cam_info *info = vp->info;
	int ret;
	if (vp->owner && vp->owner != file->private_data)
		ret = -EBUSY;
	else
		ret = vb2_poll(&vp->vb2_q, file, wait);
	return ret;
}
static int gt2440_cam_mmap(struct file *file, struct vm_area_struct *vma)
{	dprintk("gt2440_cam_mmap()\n");
	struct gt2440_cam_vp *vp = video_drvdata(file);
	int ret;
	if (vp->owner && vp->owner != file->private_data)
		ret = -EBUSY;
	else
		ret = vb2_mmap(&vp->vb2_q, vma);
	return ret;
}
static int gt2440_cam_open(struct file *file)
{	dprintk("gt2440_cam_open()\n");
	struct gt2440_cam_vp *vp = video_drvdata(file);
	struct gt2440_cam_info *info = vp->info;
	int ret;
	ret = v4l2_fh_open(file);
	if (ret < 0){
		dev_err(info->dev, "failed to open v4l2 fh");
		return ret;
	}
#ifdef GT2440_CAM_REGISTER_SENSOR_ON_OPEN 
	ret = gt2440_cam_register_sensor(info);
	if (ret < 0){
		dev_err(info->dev,"failed to register sensor\n");
		goto exit_1;
	}
#else
	;
#endif
	ret = gt2440_cam_sensor_set_power(info, 1);
	if (ret){
		dev_err(info->dev, "failed to set sensor power ");
		goto exit_2;
	}
	return ret;
exit_2:
#ifdef GT2440_CAM_REGISTER_SENSOR_ON_OPEN 
	gt2440_cam_unregister_sensor(info);
#else
	;
#endif
exit_1:
	v4l2_fh_release(file);
	return ret;
}
static int gt2440_cam_release(struct file *file)
{	dprintk("gt2440_cam_release()\n");
	struct gt2440_cam_vp *vp = video_drvdata(file);
	struct gt2440_cam_info *info = vp->info;
	int ret;
	if (vp->owner == file->private_data) {
		gt2440_cam_stop_capture(vp);
		vb2_queue_release(&vp->vb2_q);
		vp->owner = NULL;
	}
	gt2440_cam_sensor_set_power(info, 0);
#ifdef GT2440_CAM_REGISTER_SENSOR_ON_OPEN 
	gt2440_cam_unregister_sensor(info);
#else
	;
#endif
	ret = v4l2_fh_release(file);
	return ret;
}



/***** interrupt function *****/
void gt2440_cam_hw_set_lastirq(struct gt2440_cam_vp *vp, int enable)
{
	u32 addr = GT2440_CAM_REG_CICTRL(vp->id, vp->offset);
	u32 cfg;
	cfg = gt2440_cam_hw_read_reg(vp->info, addr);
	if (enable)
		cfg |= CICTRL_LASTIRQ_ENABLE;
	else
		cfg &= ~CICTRL_LASTIRQ_ENABLE;
	gt2440_cam_hw_write_reg(vp->info, addr, cfg);
}
static void gt2440_cam_prepare_dma_offset(struct gt2440_cam_vp *vp)
{
	struct gt2440_cam_frame *frame = &vp->out_frame;
	frame->dma_offset.initial = frame->rect.top * frame->f_width + frame->rect.left;
	frame->dma_offset.line = frame->f_width - (frame->rect.left + frame->rect.width);
}
static irqreturn_t gt2440_cam_irq(int irq, void *dev_id)
{	//dprintk("gt2440_cam_irq(irq=%d)\n", irq);
	struct gt2440_cam_vp *vp = dev_id;
	struct gt2440_cam_info *info = vp->info;
	unsigned int status;
	spin_lock(&info->lock);
	status = gt2440_cam_hw_get_status(vp);
	if (status & CISTATUS_OVF_MASK) {
		gt2440_cam_hw_clear_fifo_overflow(vp);
		goto exit_1;
	}
	if (vp->state & ST_VP_ABORTING) {
		if (vp->state & ST_VP_OFF) {
			vp->state &= ~(ST_VP_OFF | ST_VP_ABORTING |
				       ST_VP_LASTIRQ);
			wake_up(&vp->irq_queue);
			goto exit_1;
		} else if (vp->state & ST_VP_LASTIRQ) {
			gt2440_cam_hw_disable_capture(vp);
			gt2440_cam_hw_enable_scaler(vp, false);
			gt2440_cam_hw_set_lastirq(vp, false);
			vp->state |= ST_VP_OFF;
		} else {
			gt2440_cam_hw_set_lastirq(vp, true);
			vp->state |= ST_VP_LASTIRQ;
		}
	}
	if (!list_empty(&vp->pending_buf_q) && (vp->state & ST_VP_RUNNING) &&
	    !list_empty(&vp->active_buf_q)) {
		unsigned int index;
		struct gt2440_cam_buffer *buf;
		struct timeval *tv;
		struct timespec ts;
		index = (CISTATUS_FRAMECNT(status) + 2) & 1;
		ktime_get_ts(&ts);
		buf = gt2440_cam_active_queue_peek(vp, index);
		if (!WARN_ON(buf == NULL)) {
			tv = &buf->vb2_b.v4l2_buf.timestamp;
			tv->tv_sec = ts.tv_sec;
			tv->tv_usec = ts.tv_nsec / NSEC_PER_USEC;
			buf->vb2_b.v4l2_buf.sequence = vp->frame_sequence++;
			vb2_buffer_done(&buf->vb2_b, VB2_BUF_STATE_DONE);
			buf = gt2440_cam_pending_queue_pop(vp);
			buf->index = index;
			gt2440_cam_hw_set_output_addr(vp, &buf->paddr, index);
			gt2440_cam_hw_set_output_addr(vp, &buf->paddr, index + 2);
			gt2440_cam_active_queue_add(vp, buf);
		}
	} else if (!(vp->state & ST_VP_ABORTING) &&
		   (vp->state & ST_VP_PENDING))  {
		vp->state |= ST_VP_RUNNING;
	}
	if (vp->state & ST_VP_CONFIG) {
		gt2440_cam_prepare_dma_offset(vp);
		gt2440_cam_hw_set_camera_rect(info);
		gt2440_cam_hw_set_scaler(vp);
		gt2440_cam_hw_set_flip(vp);
		gt2440_cam_hw_set_test_pattern(info, info->test_pattern);
		if (info->variant->has_img_effect)
			gt2440_cam_hw_set_effect(info, info->colorfx,
				    info->colorfx_cb, info->colorfx_cr);
		vp->state &= ~ST_VP_CONFIG;
	}
exit_1:
	spin_unlock(&info->lock);
	return IRQ_HANDLED;
}


/***** probe and remove *****/
static int gt2440_cam_init_info(struct gt2440_cam_info *info)
{	dprintk("gt2440_cam_init_info()\n");
	int i;
	for (i = 0; i < CAM_VP_NUM; i++) {
		struct gt2440_cam_vp *vp = &info->vp[i];
		struct gt2440_cam_frame *frame = &vp->out_frame;
		vp->info = info;
		vp->id = i;
		vp->offset = info->variant->vp_offset;
		vp->fmt_flags = i ? FMT_FL_GT2440_PREVIEW :
					FMT_FL_GT2440_CODEC;
		vp->out_fmt = gt2440_cam_find_format(vp, NULL, 0); /* "YUV 4:2:2 planar, Y/Cb/Cr" */
		BUG_ON(vp->out_fmt == NULL);
		memset(frame, 0, sizeof(*frame));
		frame->f_width = CAM_DEF_WIDTH;
		frame->f_height = CAM_DEF_HEIGHT;
		frame->rect.width = CAM_DEF_WIDTH;
		frame->rect.height = CAM_DEF_HEIGHT;
		vp->scaler.enable = 1;
		vp->payload = (frame->f_width *frame->f_height *
			       vp->out_fmt->depth) / 8;
	}
	memset(&info->mbus_framefmt, 0, sizeof(info->mbus_framefmt));
	info->mbus_framefmt.width = CAM_DEF_WIDTH;
	info->mbus_framefmt.height = CAM_DEF_HEIGHT;
	info->mbus_framefmt.code  = gt2440_cam_v4l2_mbus_pixelcodes[0];
	memset(&info->rect, 0, sizeof(info->rect));
	info->rect.width = CAM_DEF_WIDTH;
	info->rect.height = CAM_DEF_HEIGHT;
	return 0;
}
static int gt2440_cam_request_irq(struct gt2440_cam_info *info)
{	dprintk("gt2440_cam_request_irq()\n");
	struct platform_device *pdev = 
		container_of(info->dev, struct platform_device, dev);
	int i = 0;
	int ret = 0;
	for (i = 0; i < CAM_VP_NUM; i++) {
		struct gt2440_cam_vp *vp = &info->vp[i];
		init_waitqueue_head(&vp->irq_queue);
		vp->irq = platform_get_irq(pdev, i);
		if (vp->irq == 0) {
			dev_err(vp->info->dev, "failed to get irq %d\n", vp->irq);
			ret = -ENXIO;
			goto exit_1;
		}
		ret = request_irq(vp->irq, gt2440_cam_irq, 0, pdev->name, vp);
		if (ret < 0) {
			dev_err(vp->info->dev, "failed to request irq: %d, error = %d\n", vp->irq, ret);
			ret = -EBUSY;
			goto exit_1;
		}
	}
	return 0;
exit_1:
	while(i--){
		free_irq(info->vp[i].irq, &info->vp[i]);
		info->vp[i].irq = 0;
	}
	return ret;	
}
static int gt2440_cam_clk_get(struct gt2440_cam_info *info)
{	dprintk("gt2440_cam_clk_get()\n");
	int ret, i;
	for (i = 0; i < CLK_MAX_NUM; i++) {
		info->clk[i] = clk_get(info->dev, gt2440_cam_clks[i]);
		if (IS_ERR(info->clk[i])) {
			dev_err(info->dev, "failed to get clock: %s\n",gt2440_cam_clks[i]);
			ret = PTR_ERR(info->clk[i]);
			goto exit_1;
		}
	}
	return 0;
exit_1:
	while(i--){
		clk_put(info->clk[i]);
		info->clk[i] = NULL;
	}
	return ret;
}
static int gt2440_cam_register_sensor(struct gt2440_cam_info *info)
{	dprintk("gt2440_cam_register_sensor()\n");
	struct gt2440_cam_platdata *pdata = info->pdata;
	struct v4l2_device *v4l2_dev = &info->v4l2_dev;
	struct v4l2_subdev *v4l2_sub;
	struct i2c_adapter *adapter;
	struct v4l2_subdev_format format;
	int ret;
	info->sensor.v4l2_sub = NULL;
	if (pdata->i2c_board_info.addr == 0){
		dev_err(info->dev, "failed to get i2c slave address %d\n",
			  pdata->i2c_board_info.addr);
		return -EINVAL;
	}
	adapter = i2c_get_adapter(pdata->i2c_bus_num);
	if (adapter == NULL) {
		dev_err(info->dev, "failed to get i2c adapter %d\n",
			  pdata->i2c_bus_num);
		return -ENXIO;
	}
	v4l2_sub = v4l2_i2c_new_subdev_board(v4l2_dev, adapter,
				       &pdata->i2c_board_info, NULL);
	if (v4l2_sub == NULL) {
		dev_err(info->dev, "failed to acquire subdev %s\n",
			  pdata->i2c_board_info.type);
		goto exit_1;
	}
	info->sensor.v4l2_sub = v4l2_sub;
	dev_info(info->dev, "good to registered sensor subdevice %s\n", v4l2_sub->name);
	format.pad = 0;
	format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
	ret = v4l2_subdev_call(v4l2_sub, pad, get_fmt, NULL, &format);
	if (ret < 0){
		dev_err(info->dev, "failed to call subdev get_fmt%s\n",
			  pdata->i2c_board_info.type);
		goto exit_2;
	}
	dev_info(info->dev, "good to get format from sensor subdev: %dx%d, %#x\n",
		  format.format.width, format.format.height,  format.format.code);
	return ret;		
exit_2:
	i2c_unregister_device(v4l2_get_subdevdata(v4l2_sub));
exit_1:
	i2c_put_adapter(adapter);
	info->sensor.v4l2_sub = NULL;
	return ret;
}
static void gt2440_cam_unregister_sensor(struct gt2440_cam_info *info)
{	dprintk("gt2440_cam_unregister_sensor()\n");
	struct v4l2_subdev *v4l2_sub = info->sensor.v4l2_sub;
	struct i2c_client *client = v4l2_sub ? v4l2_get_subdevdata(v4l2_sub) : NULL;
	struct i2c_adapter *adapter;
	if (client == NULL)
		return;
	adapter = client->adapter;
	v4l2_device_unregister_subdev(v4l2_sub);
	info->sensor.v4l2_sub = NULL;
	i2c_unregister_device(client);
	if (adapter)
		i2c_put_adapter(adapter);
}
static int gt2440_cam_register_video_node(struct gt2440_cam_info *info, int idx)
{	dprintk("gt2440_cam_register_video_node(idx=%d)\n", idx);
	struct gt2440_cam_vp *vp = &info->vp[idx];
	struct vb2_queue *vb2_q = &vp->vb2_q;
	struct video_device *video_dev = &vp->video_dev;
	struct v4l2_ctrl *ctrl;
	int ret;
	memset(video_dev, 0, sizeof(*video_dev));
	snprintf(video_dev->name, sizeof(video_dev->name), "cam-%s",
		 vp->id == 0 ? "codec" : "preview");
	video_dev->fops = &gt2440_cam_fops;
	video_dev->ioctl_ops = &gt2440_cam_ioctl_ops;
	video_dev->v4l2_dev = &info->v4l2_dev;
	video_dev->minor = -1;
	video_dev->release = video_device_release_empty;
	video_dev->lock = &info->mutex;
	vp->reqbufs_count = 0;
	INIT_LIST_HEAD(&vp->pending_buf_q);
	INIT_LIST_HEAD(&vp->active_buf_q);
	memset(vb2_q, 0, sizeof(*vb2_q));
	vb2_q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	vb2_q->io_modes = VB2_MMAP | VB2_USERPTR;
	vb2_q->ops = &gt2440_cam_vb2_ops;
	vb2_q->mem_ops = &vb2_dma_contig_memops;
	vb2_q->buf_struct_size = sizeof(struct gt2440_cam_buffer);
	vb2_q->drv_priv = vp;
	vb2_queue_init(vb2_q);
	video_set_drvdata(video_dev, vp);
	set_bit(V4L2_FL_USE_FH_PRIO, &video_dev->flags);
	v4l2_ctrl_handler_init(&vp->ctrl_handler, 1);
	ctrl = v4l2_ctrl_new_std(&vp->ctrl_handler, &gt2440_cam_ctrl_ops,
				 V4L2_CID_HFLIP, 0, 1, 1, 0);
	if (ctrl)
		ctrl->priv = vp;
	ctrl = v4l2_ctrl_new_std(&vp->ctrl_handler, &gt2440_cam_ctrl_ops,
				 V4L2_CID_VFLIP, 0, 1, 1, 0);
	if (ctrl)
		ctrl->priv = vp;
	ret = vp->ctrl_handler.error;
	if (ret < 0){
		dev_err(info->dev,"failed to new ctrl\n");
		goto exit_1;
	}
	video_dev->ctrl_handler = &vp->ctrl_handler;
	ret = video_register_device(video_dev, VFL_TYPE_GRABBER, -1);
	if (ret){
		dev_err(info->dev,"failed to register video device\n");
		goto exit_1;
	}
	v4l2_info(&info->v4l2_dev, "Camera V4L2 device registered %s as /dev/%s\n",
		  video_dev->name, video_device_node_name(video_dev));
	return 0;
exit_1:
	v4l2_ctrl_handler_free(&vp->ctrl_handler);
	return ret;
}
static void gt2440_cam_unregister_video_node(struct gt2440_cam_info *info, int idx)
{	dprintk("gt2440_cam_unregister_video_node()\n");
	struct gt2440_cam_vp *vp = &info->vp[idx];
	struct video_device *video_dev = &vp->video_dev;
	if (video_is_registered(video_dev)) {
		v4l2_info(&info->v4l2_dev, 
			"Camera V4L2 device unregistering for %s\n",
			video_device_node_name(video_dev));
		video_unregister_device(video_dev);
		v4l2_ctrl_handler_free(&vp->ctrl_handler);
	}
}
static int gt2440_cam_probe(struct platform_device *pdev)
{
	struct gt2440_cam_info *info;
	struct gt2440_cam_platdata *pdata;
	struct resource *res;
	int size;
	int ret;
	int i = 0;
	pdata = pdev->dev.platform_data;
	if (pdata == NULL) {
		dev_err(&pdev->dev,"failed to get platform data\n");
		return -EINVAL;
	}
	info = kzalloc(sizeof(struct gt2440_cam_info), GFP_KERNEL);
	if(info == NULL){
		dev_err(&pdev->dev,"failed to allocate struct gt2440_cam_info\n");
		return -ENOMEM;
	}
	info->dev = &pdev->dev;
	info->pdata = pdata;
	info->variant = &gt2440_cam_var;
	mutex_init(&info->mutex);
	spin_lock_init(&info->lock);
	gt2440_cam_init_info(info);
	platform_set_drvdata(pdev, info);
	
	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
	if (res == NULL) {
		dev_err(&pdev->dev, "failed to get memory resource\n");
		ret = -ENXIO;
		goto	exit_1;
	}
	size = resource_size(res);
	info->res = request_mem_region(res->start, size, pdev->name);
	if (info->res == NULL) {
		dev_err(&pdev->dev, "failed to get memory region\n");
		ret = -ENOENT;
		goto exit_1;
	}
	info->regs = ioremap(res->start, size);
	if (info->regs == NULL) {
		dev_err(&pdev->dev, "failed to ioremap memory region\n");
		ret = -ENXIO;
		goto exit_2;
	}
	ret = gt2440_cam_request_irq(info);
	if(ret){
		goto exit_3;
	}
	if (pdata->gpio_get){
		ret = pdata->gpio_get();
		if(ret){
			dev_err(&pdev->dev, "failed to get GPIO: error = %d \n", ret);
			goto exit_4;
		}
	}
	ret = gt2440_cam_clk_get(info);
	if (ret < 0){
		dev_err(&pdev->dev, "failed to get clk: error = %d \n", ret);
		goto exit_5;
	}
	clk_set_rate(info->clk[CLK_CAM],
			info->pdata->clock_frequency);
	dev_info(&pdev->dev, "good to get sensor clock frequency: %lu\n",
		 clk_get_rate(info->clk[CLK_CAM]));
	ret = v4l2_ctrl_handler_init(&info->ctrl_handler, 3);
	if (ret < 0) {
		dev_err(info->dev,"failed to init ctrl handler\n");
		goto exit_6;
	}
	info->v4l2_dev.ctrl_handler = &info->ctrl_handler;

	info->alloc_ctx = vb2_dma_contig_init_ctx(info->dev);
	if (IS_ERR(info->alloc_ctx)) {
		dev_err(info->dev,"failed to allocate dma contig memory\n");
		ret = PTR_ERR(info->alloc_ctx);
		goto exit_7;
	}
	/* critical code: don't use sizeof() !!! */
	strlcpy(info->v4l2_dev.name, "cam", strlen("cam"));
	ret = v4l2_device_register(info->dev, &info->v4l2_dev);
	if (ret < 0){
		dev_err(info->dev,"failed to register v4l2 device\n");
		goto exit_8;
	}
#ifdef GT2440_CAM_REGISTER_SENSOR_ON_OPEN 
	;
#else
	ret = gt2440_cam_register_sensor(info);
	if (ret < 0){
		dev_err(info->dev,"failed to register sensor\n");
		goto exit_9;
	}
#endif
	ret = gt2440_cam_register_video_node(info, VP_CODEC);
	if (ret < 0){
		dev_err(&pdev->dev, "failed to register codec video node : error = %d \n", ret);
		goto exit_10;
	}
	ret = gt2440_cam_register_video_node(info, VP_PREVIEW);
	if (ret < 0){
		dev_err(&pdev->dev, "failed to register preview video node : error = %d \n", ret);
		goto exit_11;
	}
	return 0;
exit_11:
	gt2440_cam_unregister_video_node(info, VP_CODEC);
exit_10:
#ifdef GT2440_CAM_REGISTER_SENSOR_ON_OPEN 
	;
#else
	gt2440_cam_unregister_sensor(info);
#endif	
exit_9:
	v4l2_device_unregister(&info->v4l2_dev);
exit_8:
	vb2_dma_contig_cleanup_ctx(info->alloc_ctx);
	info->alloc_ctx = NULL;
exit_7:
	v4l2_ctrl_handler_free(&info->ctrl_handler);
	info->v4l2_dev.ctrl_handler = NULL;
exit_6:
	for (i = 0; i < CLK_MAX_NUM; i++) {
		clk_put(info->clk[i]);
		info->clk[i] = NULL;
	}
exit_5:
	if (pdata->gpio_put)
		pdata->gpio_put();
exit_4:
	for (i = 0; i < CAM_VP_NUM; i++) {
		free_irq(info->vp[i].irq, &info->vp[i]);
		info->vp[i].irq = 0;
	}
exit_3:
	iounmap(info->regs);
	info->regs = NULL;
exit_2:
	release_mem_region(info->res->start, resource_size(info->res));
	info->res = NULL;
exit_1:
	kfree(info);
	platform_set_drvdata(pdev, NULL);
	return ret;
}
static int gt2440_cam_remove(struct platform_device *pdev)
{
	struct gt2440_cam_info *info = platform_get_drvdata(pdev);
	struct gt2440_cam_platdata *pdata = info->pdata;
	int i = 0;
	if(info == NULL)
		return 0;
	gt2440_cam_unregister_video_node(info, VP_PREVIEW);
	gt2440_cam_unregister_video_node(info, VP_CODEC);

#ifdef GT2440_CAM_REGISTER_SENSOR_ON_OPEN 
	;
#else
	gt2440_cam_unregister_sensor(info);
#endif	
	v4l2_device_unregister(&info->v4l2_dev);
	if(info->alloc_ctx != NULL){
		vb2_dma_contig_cleanup_ctx(info->alloc_ctx);
		info->alloc_ctx = NULL;
	}
	v4l2_ctrl_handler_free(&info->ctrl_handler);
	for (i = 0; i < CLK_MAX_NUM; i++) 
		if(info->clk[i] != NULL){
			clk_put(info->clk[i]);
			info->clk[i] = NULL;
		}
	if (pdata->gpio_put)
		pdata->gpio_put();
	for (i = 0; i < CAM_VP_NUM; i++) 
		if(info->vp[i].irq != 0){
			free_irq(info->vp[i].irq, &info->vp[i]);
			info->vp[i].irq = 0;
		}
	if(info->regs != NULL){
		iounmap(info->regs);
		info->regs = NULL;
	}
	if(info->res != NULL){
		release_mem_region(info->res->start, resource_size(info->res));
		info->res = NULL;
	}
	kfree(info);
	platform_set_drvdata(pdev, NULL);
	return 0;
}
/***** gt2440 board probe and remove function *****/
static int gt2440_board_cam_probe(struct platform_device *pdev)
{	dprintk("gt2440_board_cam_probe(id=%d)\n", pdev->id);
	return gt2440_cam_probe(pdev);
}
static int gt2440_board_cam_remove(struct platform_device *pdev)
{	dprintk("gt2440_board_cam_remove(id=%d)\n", pdev->id);
	return gt2440_cam_remove(pdev);
}
/***** platform driver data structure *****/
static struct platform_driver gt2440_cam_platform_driver = {
	.probe		= gt2440_board_cam_probe,
	.remove		= gt2440_board_cam_remove,
	.driver		= {
		.name	= PLAT_DEVICE_NAME, /* "gt2440-cam" */
		.owner	= THIS_MODULE,
	},
};
/***** init and exit ****/
static int __init gt2440_cam_init(void)
{	dprintk("gt2440_cam_init()\n");
	return platform_driver_register(&gt2440_cam_platform_driver);
}
static void __exit gt2440_cam_exit(void)
{	dprintk("gt2440_cam_exit()\n");
	platform_driver_unregister(&gt2440_cam_platform_driver);
}
module_init(gt2440_cam_init);
module_exit(gt2440_cam_exit);
MODULE_DESCRIPTION("GT2440 Camera Interface Device Driver");
MODULE_AUTHOR("Liguang13579<1659890447@qq.com>");
MODULE_LICENSE("GPL v2");