From: Mauro Carvalho Chehab <mchehab@redhat.com>
Date: Sat, 6 Oct 2012 01:32:05 +0000 (-0300)
Subject: Merge branch 'samsung_platform_data' into staging/for_v3.7
X-Git-Tag: firefly_0821_release~3680^2~275^2~967
X-Git-Url: http://demsky.eecs.uci.edu/git/?a=commitdiff_plain;h=eabe7b01c249c9d8166a1a10bb6effce2b3de665;p=firefly-linux-kernel-4.4.55.git

Merge branch 'samsung_platform_data' into staging/for_v3.7

* samsung_platform_data:
  ARM: samsung: move platform_data definitions
  ARM: orion: move platform_data definitions
  ARM: nomadik: move platform_data definitions
  ARM: w90x900: move platform_data definitions
  ARM: vt8500: move platform_data definitions
  ARM: tegra: move sdhci platform_data definition
  ARM: sa1100: move platform_data definitions
  ARM: pxa: move platform_data definitions
  ARM: netx: move platform_data definitions
  ARM: msm: move platform_data definitions
  ARM: imx: move platform_data definitions
  ARM: ep93xx: move platform_data definitions
  ARM: davinci: move platform_data definitions
  ARM: at91: move platform_data definitions
---

eabe7b01c249c9d8166a1a10bb6effce2b3de665
diff --cc arch/arm/mach-davinci/board-da850-evm.c
index bc6b8c7b86c3,1295e616ceee..32ee3f895967
--- a/arch/arm/mach-davinci/board-da850-evm.c
+++ b/arch/arm/mach-davinci/board-da850-evm.c
@@@ -40,14 -40,11 +40,14 @@@
  
  #include <mach/cp_intc.h>
  #include <mach/da8xx.h>
- #include <mach/nand.h>
+ #include <linux/platform_data/mtd-davinci.h>
  #include <mach/mux.h>
- #include <mach/aemif.h>
- #include <mach/spi.h>
+ #include <linux/platform_data/mtd-davinci-aemif.h>
+ #include <linux/platform_data/spi-davinci.h>
  
 +#include <media/tvp514x.h>
 +#include <media/adv7343.h>
 +
  #define DA850_EVM_PHY_ID		"davinci_mdio-0:00"
  #define DA850_LCD_PWR_PIN		GPIO_TO_PIN(2, 8)
  #define DA850_LCD_BL_PIN		GPIO_TO_PIN(2, 15)
diff --cc arch/arm/mach-davinci/include/mach/da8xx.h
index 8eecbce7febc,33e78ae2a254..13d229575757
--- a/arch/arm/mach-davinci/include/mach/da8xx.h
+++ b/arch/arm/mach-davinci/include/mach/da8xx.h
@@@ -20,15 -19,13 +20,15 @@@
  
  #include <mach/serial.h>
  #include <mach/edma.h>
- #include <mach/i2c.h>
  #include <mach/asp.h>
- #include <mach/mmc.h>
- #include <mach/usb.h>
  #include <mach/pm.h>
- #include <mach/spi.h>
+ #include <linux/platform_data/i2c-davinci.h>
+ #include <linux/platform_data/mmc-davinci.h>
+ #include <linux/platform_data/usb-davinci.h>
+ #include <linux/platform_data/spi-davinci.h>
  
 +#include <media/davinci/vpif_types.h>
 +
  extern void __iomem *da8xx_syscfg0_base;
  extern void __iomem *da8xx_syscfg1_base;
  
diff --cc arch/arm/mach-exynos/mach-origen.c
index a54ce4ef7057,4a531dccd0c6..0b8d24e27e74
--- a/arch/arm/mach-exynos/mach-origen.c
+++ b/arch/arm/mach-exynos/mach-origen.c
@@@ -42,9 -42,8 +42,9 @@@
  #include <plat/backlight.h>
  #include <plat/fb.h>
  #include <plat/mfc.h>
 +#include <plat/hdmi.h>
  
- #include <mach/ohci.h>
+ #include <linux/platform_data/usb-exynos.h>
  #include <mach/map.h>
  
  #include <drm/exynos_drm.h>
diff --cc arch/arm/mach-exynos/mach-smdkv310.c
index 73f2bce097e1,fa2ada6c9ca7..1b864ee387cd
--- a/arch/arm/mach-exynos/mach-smdkv310.c
+++ b/arch/arm/mach-exynos/mach-smdkv310.c
@@@ -38,12 -38,11 +38,12 @@@
  #include <plat/gpio-cfg.h>
  #include <plat/backlight.h>
  #include <plat/mfc.h>
- #include <plat/ehci.h>
+ #include <linux/platform_data/usb-ehci-s5p.h>
  #include <plat/clock.h>
 +#include <plat/hdmi.h>
  
  #include <mach/map.h>
- #include <mach/ohci.h>
+ #include <linux/platform_data/usb-exynos.h>
  
  #include <drm/exynos_drm.h>
  #include "common.h"
diff --cc drivers/media/platform/davinci/vpbe_venc.c
index 86d47c39033d,000000000000..aed7369b962a
mode 100644,000000..100644
--- a/drivers/media/platform/davinci/vpbe_venc.c
+++ b/drivers/media/platform/davinci/vpbe_venc.c
@@@ -1,707 -1,0 +1,707 @@@
 +/*
 + * Copyright (C) 2010 Texas Instruments Inc
 + *
 + * This program is free software; you can redistribute it and/or modify
 + * it under the terms of the GNU General Public License as published by
 + * the Free Software Foundation version 2.
 + *
 + * This program is distributed in the hope that it will be useful,
 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 + * GNU General Public License for more details.
 + *
 + * You should have received a copy of the GNU General Public License
 + * along with this program; if not, write to the Free Software
 + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 + */
 +#include <linux/module.h>
 +#include <linux/kernel.h>
 +#include <linux/init.h>
 +#include <linux/ctype.h>
 +#include <linux/delay.h>
 +#include <linux/device.h>
 +#include <linux/interrupt.h>
 +#include <linux/platform_device.h>
 +#include <linux/videodev2.h>
 +#include <linux/slab.h>
 +
 +#include <mach/hardware.h>
 +#include <mach/mux.h>
- #include <mach/i2c.h>
++#include <linux/platform_data/i2c-davinci.h>
 +
 +#include <linux/io.h>
 +
 +#include <media/davinci/vpbe_types.h>
 +#include <media/davinci/vpbe_venc.h>
 +#include <media/davinci/vpss.h>
 +#include <media/v4l2-device.h>
 +
 +#include "vpbe_venc_regs.h"
 +
 +#define MODULE_NAME	VPBE_VENC_SUBDEV_NAME
 +
 +static int debug = 2;
 +module_param(debug, int, 0644);
 +MODULE_PARM_DESC(debug, "Debug level 0-2");
 +
 +struct venc_state {
 +	struct v4l2_subdev sd;
 +	struct venc_callback *callback;
 +	struct venc_platform_data *pdata;
 +	struct device *pdev;
 +	u32 output;
 +	v4l2_std_id std;
 +	spinlock_t lock;
 +	void __iomem *venc_base;
 +	void __iomem *vdaccfg_reg;
 +};
 +
 +static inline struct venc_state *to_state(struct v4l2_subdev *sd)
 +{
 +	return container_of(sd, struct venc_state, sd);
 +}
 +
 +static inline u32 venc_read(struct v4l2_subdev *sd, u32 offset)
 +{
 +	struct venc_state *venc = to_state(sd);
 +
 +	return readl(venc->venc_base + offset);
 +}
 +
 +static inline u32 venc_write(struct v4l2_subdev *sd, u32 offset, u32 val)
 +{
 +	struct venc_state *venc = to_state(sd);
 +
 +	writel(val, (venc->venc_base + offset));
 +
 +	return val;
 +}
 +
 +static inline u32 venc_modify(struct v4l2_subdev *sd, u32 offset,
 +				 u32 val, u32 mask)
 +{
 +	u32 new_val = (venc_read(sd, offset) & ~mask) | (val & mask);
 +
 +	venc_write(sd, offset, new_val);
 +
 +	return new_val;
 +}
 +
 +static inline u32 vdaccfg_write(struct v4l2_subdev *sd, u32 val)
 +{
 +	struct venc_state *venc = to_state(sd);
 +
 +	writel(val, venc->vdaccfg_reg);
 +
 +	val = readl(venc->vdaccfg_reg);
 +
 +	return val;
 +}
 +
 +#define VDAC_COMPONENT	0x543
 +#define VDAC_S_VIDEO	0x210
 +/* This function sets the dac of the VPBE for various outputs
 + */
 +static int venc_set_dac(struct v4l2_subdev *sd, u32 out_index)
 +{
 +	switch (out_index) {
 +	case 0:
 +		v4l2_dbg(debug, 1, sd, "Setting output to Composite\n");
 +		venc_write(sd, VENC_DACSEL, 0);
 +		break;
 +	case 1:
 +		v4l2_dbg(debug, 1, sd, "Setting output to Component\n");
 +		venc_write(sd, VENC_DACSEL, VDAC_COMPONENT);
 +		break;
 +	case 2:
 +		v4l2_dbg(debug, 1, sd, "Setting output to S-video\n");
 +		venc_write(sd, VENC_DACSEL, VDAC_S_VIDEO);
 +		break;
 +	default:
 +		return -EINVAL;
 +	}
 +
 +	return 0;
 +}
 +
 +static void venc_enabledigitaloutput(struct v4l2_subdev *sd, int benable)
 +{
 +	struct venc_state *venc = to_state(sd);
 +	struct venc_platform_data *pdata = venc->pdata;
 +	v4l2_dbg(debug, 2, sd, "venc_enabledigitaloutput\n");
 +
 +	if (benable) {
 +		venc_write(sd, VENC_VMOD, 0);
 +		venc_write(sd, VENC_CVBS, 0);
 +		venc_write(sd, VENC_LCDOUT, 0);
 +		venc_write(sd, VENC_HSPLS, 0);
 +		venc_write(sd, VENC_HSTART, 0);
 +		venc_write(sd, VENC_HVALID, 0);
 +		venc_write(sd, VENC_HINT, 0);
 +		venc_write(sd, VENC_VSPLS, 0);
 +		venc_write(sd, VENC_VSTART, 0);
 +		venc_write(sd, VENC_VVALID, 0);
 +		venc_write(sd, VENC_VINT, 0);
 +		venc_write(sd, VENC_YCCCTL, 0);
 +		venc_write(sd, VENC_DACSEL, 0);
 +
 +	} else {
 +		venc_write(sd, VENC_VMOD, 0);
 +		/* disable VCLK output pin enable */
 +		venc_write(sd, VENC_VIDCTL, 0x141);
 +
 +		/* Disable output sync pins */
 +		venc_write(sd, VENC_SYNCCTL, 0);
 +
 +		/* Disable DCLOCK */
 +		venc_write(sd, VENC_DCLKCTL, 0);
 +		venc_write(sd, VENC_DRGBX1, 0x0000057C);
 +
 +		/* Disable LCD output control (accepting default polarity) */
 +		venc_write(sd, VENC_LCDOUT, 0);
 +		if (pdata->venc_type != VPBE_VERSION_3)
 +			venc_write(sd, VENC_CMPNT, 0x100);
 +		venc_write(sd, VENC_HSPLS, 0);
 +		venc_write(sd, VENC_HINT, 0);
 +		venc_write(sd, VENC_HSTART, 0);
 +		venc_write(sd, VENC_HVALID, 0);
 +
 +		venc_write(sd, VENC_VSPLS, 0);
 +		venc_write(sd, VENC_VINT, 0);
 +		venc_write(sd, VENC_VSTART, 0);
 +		venc_write(sd, VENC_VVALID, 0);
 +
 +		venc_write(sd, VENC_HSDLY, 0);
 +		venc_write(sd, VENC_VSDLY, 0);
 +
 +		venc_write(sd, VENC_YCCCTL, 0);
 +		venc_write(sd, VENC_VSTARTA, 0);
 +
 +		/* Set OSD clock and OSD Sync Adavance registers */
 +		venc_write(sd, VENC_OSDCLK0, 1);
 +		venc_write(sd, VENC_OSDCLK1, 2);
 +	}
 +}
 +
 +#define VDAC_CONFIG_SD_V3	0x0E21A6B6
 +#define VDAC_CONFIG_SD_V2	0x081141CF
 +/*
 + * setting NTSC mode
 + */
 +static int venc_set_ntsc(struct v4l2_subdev *sd)
 +{
 +	u32 val;
 +	struct venc_state *venc = to_state(sd);
 +	struct venc_platform_data *pdata = venc->pdata;
 +
 +	v4l2_dbg(debug, 2, sd, "venc_set_ntsc\n");
 +
 +	/* Setup clock at VPSS & VENC for SD */
 +	vpss_enable_clock(VPSS_VENC_CLOCK_SEL, 1);
 +	if (pdata->setup_clock(VPBE_ENC_STD, V4L2_STD_525_60) < 0)
 +		return -EINVAL;
 +
 +	venc_enabledigitaloutput(sd, 0);
 +
 +	if (pdata->venc_type == VPBE_VERSION_3) {
 +		venc_write(sd, VENC_CLKCTL, 0x01);
 +		venc_write(sd, VENC_VIDCTL, 0);
 +		val = vdaccfg_write(sd, VDAC_CONFIG_SD_V3);
 +	} else if (pdata->venc_type == VPBE_VERSION_2) {
 +		venc_write(sd, VENC_CLKCTL, 0x01);
 +		venc_write(sd, VENC_VIDCTL, 0);
 +		vdaccfg_write(sd, VDAC_CONFIG_SD_V2);
 +	} else {
 +		/* to set VENC CLK DIV to 1 - final clock is 54 MHz */
 +		venc_modify(sd, VENC_VIDCTL, 0, 1 << 1);
 +		/* Set REC656 Mode */
 +		venc_write(sd, VENC_YCCCTL, 0x1);
 +		venc_modify(sd, VENC_VDPRO, 0, VENC_VDPRO_DAFRQ);
 +		venc_modify(sd, VENC_VDPRO, 0, VENC_VDPRO_DAUPS);
 +	}
 +
 +	venc_write(sd, VENC_VMOD, 0);
 +	venc_modify(sd, VENC_VMOD, (1 << VENC_VMOD_VIE_SHIFT),
 +			VENC_VMOD_VIE);
 +	venc_modify(sd, VENC_VMOD, (0 << VENC_VMOD_VMD), VENC_VMOD_VMD);
 +	venc_modify(sd, VENC_VMOD, (0 << VENC_VMOD_TVTYP_SHIFT),
 +			VENC_VMOD_TVTYP);
 +	venc_write(sd, VENC_DACTST, 0x0);
 +	venc_modify(sd, VENC_VMOD, VENC_VMOD_VENC, VENC_VMOD_VENC);
 +
 +	return 0;
 +}
 +
 +/*
 + * setting PAL mode
 + */
 +static int venc_set_pal(struct v4l2_subdev *sd)
 +{
 +	struct venc_state *venc = to_state(sd);
 +	struct venc_platform_data *pdata = venc->pdata;
 +
 +	v4l2_dbg(debug, 2, sd, "venc_set_pal\n");
 +
 +	/* Setup clock at VPSS & VENC for SD */
 +	vpss_enable_clock(VPSS_VENC_CLOCK_SEL, 1);
 +	if (venc->pdata->setup_clock(VPBE_ENC_STD, V4L2_STD_625_50) < 0)
 +		return -EINVAL;
 +
 +	venc_enabledigitaloutput(sd, 0);
 +
 +	if (pdata->venc_type == VPBE_VERSION_3) {
 +		venc_write(sd, VENC_CLKCTL, 0x1);
 +		venc_write(sd, VENC_VIDCTL, 0);
 +		vdaccfg_write(sd, VDAC_CONFIG_SD_V3);
 +	} else if (pdata->venc_type == VPBE_VERSION_2) {
 +		venc_write(sd, VENC_CLKCTL, 0x1);
 +		venc_write(sd, VENC_VIDCTL, 0);
 +		vdaccfg_write(sd, VDAC_CONFIG_SD_V2);
 +	} else {
 +		/* to set VENC CLK DIV to 1 - final clock is 54 MHz */
 +		venc_modify(sd, VENC_VIDCTL, 0, 1 << 1);
 +		/* Set REC656 Mode */
 +		venc_write(sd, VENC_YCCCTL, 0x1);
 +	}
 +
 +	venc_modify(sd, VENC_SYNCCTL, 1 << VENC_SYNCCTL_OVD_SHIFT,
 +			VENC_SYNCCTL_OVD);
 +	venc_write(sd, VENC_VMOD, 0);
 +	venc_modify(sd, VENC_VMOD,
 +			(1 << VENC_VMOD_VIE_SHIFT),
 +			VENC_VMOD_VIE);
 +	venc_modify(sd, VENC_VMOD,
 +			(0 << VENC_VMOD_VMD), VENC_VMOD_VMD);
 +	venc_modify(sd, VENC_VMOD,
 +			(1 << VENC_VMOD_TVTYP_SHIFT),
 +			VENC_VMOD_TVTYP);
 +	venc_write(sd, VENC_DACTST, 0x0);
 +	venc_modify(sd, VENC_VMOD, VENC_VMOD_VENC, VENC_VMOD_VENC);
 +
 +	return 0;
 +}
 +
 +#define VDAC_CONFIG_HD_V2	0x081141EF
 +/*
 + * venc_set_480p59_94
 + *
 + * This function configures the video encoder to EDTV(525p) component setting.
 + */
 +static int venc_set_480p59_94(struct v4l2_subdev *sd)
 +{
 +	struct venc_state *venc = to_state(sd);
 +	struct venc_platform_data *pdata = venc->pdata;
 +
 +	v4l2_dbg(debug, 2, sd, "venc_set_480p59_94\n");
 +	if ((pdata->venc_type != VPBE_VERSION_1) &&
 +	    (pdata->venc_type != VPBE_VERSION_2))
 +		return -EINVAL;
 +
 +	/* Setup clock at VPSS & VENC for SD */
 +	if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 27000000) < 0)
 +		return -EINVAL;
 +
 +	venc_enabledigitaloutput(sd, 0);
 +
 +	if (pdata->venc_type == VPBE_VERSION_2)
 +		vdaccfg_write(sd, VDAC_CONFIG_HD_V2);
 +	venc_write(sd, VENC_OSDCLK0, 0);
 +	venc_write(sd, VENC_OSDCLK1, 1);
 +
 +	if (pdata->venc_type == VPBE_VERSION_1) {
 +		venc_modify(sd, VENC_VDPRO, VENC_VDPRO_DAFRQ,
 +			    VENC_VDPRO_DAFRQ);
 +		venc_modify(sd, VENC_VDPRO, VENC_VDPRO_DAUPS,
 +			    VENC_VDPRO_DAUPS);
 +	}
 +
 +	venc_write(sd, VENC_VMOD, 0);
 +	venc_modify(sd, VENC_VMOD, (1 << VENC_VMOD_VIE_SHIFT),
 +		    VENC_VMOD_VIE);
 +	venc_modify(sd, VENC_VMOD, VENC_VMOD_HDMD, VENC_VMOD_HDMD);
 +	venc_modify(sd, VENC_VMOD, (HDTV_525P << VENC_VMOD_TVTYP_SHIFT),
 +		    VENC_VMOD_TVTYP);
 +	venc_modify(sd, VENC_VMOD, VENC_VMOD_VDMD_YCBCR8 <<
 +		    VENC_VMOD_VDMD_SHIFT, VENC_VMOD_VDMD);
 +
 +	venc_modify(sd, VENC_VMOD, VENC_VMOD_VENC, VENC_VMOD_VENC);
 +
 +	return 0;
 +}
 +
 +/*
 + * venc_set_625p
 + *
 + * This function configures the video encoder to HDTV(625p) component setting
 + */
 +static int venc_set_576p50(struct v4l2_subdev *sd)
 +{
 +	struct venc_state *venc = to_state(sd);
 +	struct venc_platform_data *pdata = venc->pdata;
 +
 +	v4l2_dbg(debug, 2, sd, "venc_set_576p50\n");
 +
 +	if ((pdata->venc_type != VPBE_VERSION_1) &&
 +	  (pdata->venc_type != VPBE_VERSION_2))
 +		return -EINVAL;
 +	/* Setup clock at VPSS & VENC for SD */
 +	if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 27000000) < 0)
 +		return -EINVAL;
 +
 +	venc_enabledigitaloutput(sd, 0);
 +
 +	if (pdata->venc_type == VPBE_VERSION_2)
 +		vdaccfg_write(sd, VDAC_CONFIG_HD_V2);
 +
 +	venc_write(sd, VENC_OSDCLK0, 0);
 +	venc_write(sd, VENC_OSDCLK1, 1);
 +
 +	if (pdata->venc_type == VPBE_VERSION_1) {
 +		venc_modify(sd, VENC_VDPRO, VENC_VDPRO_DAFRQ,
 +			    VENC_VDPRO_DAFRQ);
 +		venc_modify(sd, VENC_VDPRO, VENC_VDPRO_DAUPS,
 +			    VENC_VDPRO_DAUPS);
 +	}
 +
 +	venc_write(sd, VENC_VMOD, 0);
 +	venc_modify(sd, VENC_VMOD, (1 << VENC_VMOD_VIE_SHIFT),
 +		    VENC_VMOD_VIE);
 +	venc_modify(sd, VENC_VMOD, VENC_VMOD_HDMD, VENC_VMOD_HDMD);
 +	venc_modify(sd, VENC_VMOD, (HDTV_625P << VENC_VMOD_TVTYP_SHIFT),
 +		    VENC_VMOD_TVTYP);
 +
 +	venc_modify(sd, VENC_VMOD, VENC_VMOD_VDMD_YCBCR8 <<
 +		    VENC_VMOD_VDMD_SHIFT, VENC_VMOD_VDMD);
 +	venc_modify(sd, VENC_VMOD, VENC_VMOD_VENC, VENC_VMOD_VENC);
 +
 +	return 0;
 +}
 +
 +/*
 + * venc_set_720p60_internal - Setup 720p60 in venc for dm365 only
 + */
 +static int venc_set_720p60_internal(struct v4l2_subdev *sd)
 +{
 +	struct venc_state *venc = to_state(sd);
 +	struct venc_platform_data *pdata = venc->pdata;
 +
 +	if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 74250000) < 0)
 +		return -EINVAL;
 +
 +	venc_enabledigitaloutput(sd, 0);
 +
 +	venc_write(sd, VENC_OSDCLK0, 0);
 +	venc_write(sd, VENC_OSDCLK1, 1);
 +
 +	venc_write(sd, VENC_VMOD, 0);
 +	/* DM365 component HD mode */
 +	venc_modify(sd, VENC_VMOD, (1 << VENC_VMOD_VIE_SHIFT),
 +	    VENC_VMOD_VIE);
 +	venc_modify(sd, VENC_VMOD, VENC_VMOD_HDMD, VENC_VMOD_HDMD);
 +	venc_modify(sd, VENC_VMOD, (HDTV_720P << VENC_VMOD_TVTYP_SHIFT),
 +		    VENC_VMOD_TVTYP);
 +	venc_modify(sd, VENC_VMOD, VENC_VMOD_VENC, VENC_VMOD_VENC);
 +	venc_write(sd, VENC_XHINTVL, 0);
 +	return 0;
 +}
 +
 +/*
 + * venc_set_1080i30_internal - Setup 1080i30 in venc for dm365 only
 + */
 +static int venc_set_1080i30_internal(struct v4l2_subdev *sd)
 +{
 +	struct venc_state *venc = to_state(sd);
 +	struct venc_platform_data *pdata = venc->pdata;
 +
 +	if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 74250000) < 0)
 +		return -EINVAL;
 +
 +	venc_enabledigitaloutput(sd, 0);
 +
 +	venc_write(sd, VENC_OSDCLK0, 0);
 +	venc_write(sd, VENC_OSDCLK1, 1);
 +
 +
 +	venc_write(sd, VENC_VMOD, 0);
 +	/* DM365 component HD mode */
 +	venc_modify(sd, VENC_VMOD, (1 << VENC_VMOD_VIE_SHIFT),
 +		    VENC_VMOD_VIE);
 +	venc_modify(sd, VENC_VMOD, VENC_VMOD_HDMD, VENC_VMOD_HDMD);
 +	venc_modify(sd, VENC_VMOD, (HDTV_1080I << VENC_VMOD_TVTYP_SHIFT),
 +		    VENC_VMOD_TVTYP);
 +	venc_modify(sd, VENC_VMOD, VENC_VMOD_VENC, VENC_VMOD_VENC);
 +	venc_write(sd, VENC_XHINTVL, 0);
 +	return 0;
 +}
 +
 +static int venc_s_std_output(struct v4l2_subdev *sd, v4l2_std_id norm)
 +{
 +	v4l2_dbg(debug, 1, sd, "venc_s_std_output\n");
 +
 +	if (norm & V4L2_STD_525_60)
 +		return venc_set_ntsc(sd);
 +	else if (norm & V4L2_STD_625_50)
 +		return venc_set_pal(sd);
 +
 +	return -EINVAL;
 +}
 +
 +static int venc_s_dv_timings(struct v4l2_subdev *sd,
 +			    struct v4l2_dv_timings *dv_timings)
 +{
 +	struct venc_state *venc = to_state(sd);
 +	u32 height = dv_timings->bt.height;
 +	int ret;
 +
 +	v4l2_dbg(debug, 1, sd, "venc_s_dv_timings\n");
 +
 +	if (height == 576)
 +		return venc_set_576p50(sd);
 +	else if (height == 480)
 +		return venc_set_480p59_94(sd);
 +	else if ((height == 720) &&
 +			(venc->pdata->venc_type == VPBE_VERSION_2)) {
 +		/* TBD setup internal 720p mode here */
 +		ret = venc_set_720p60_internal(sd);
 +		/* for DM365 VPBE, there is DAC inside */
 +		vdaccfg_write(sd, VDAC_CONFIG_HD_V2);
 +		return ret;
 +	} else if ((height == 1080) &&
 +		(venc->pdata->venc_type == VPBE_VERSION_2)) {
 +		/* TBD setup internal 1080i mode here */
 +		ret = venc_set_1080i30_internal(sd);
 +		/* for DM365 VPBE, there is DAC inside */
 +		vdaccfg_write(sd, VDAC_CONFIG_HD_V2);
 +		return ret;
 +	}
 +	return -EINVAL;
 +}
 +
 +static int venc_s_routing(struct v4l2_subdev *sd, u32 input, u32 output,
 +			  u32 config)
 +{
 +	struct venc_state *venc = to_state(sd);
 +	int ret;
 +
 +	v4l2_dbg(debug, 1, sd, "venc_s_routing\n");
 +
 +	ret = venc_set_dac(sd, output);
 +	if (!ret)
 +		venc->output = output;
 +
 +	return ret;
 +}
 +
 +static long venc_ioctl(struct v4l2_subdev *sd,
 +			unsigned int cmd,
 +			void *arg)
 +{
 +	u32 val;
 +
 +	switch (cmd) {
 +	case VENC_GET_FLD:
 +		val = venc_read(sd, VENC_VSTAT);
 +		*((int *)arg) = ((val & VENC_VSTAT_FIDST) ==
 +		VENC_VSTAT_FIDST);
 +		break;
 +	default:
 +		v4l2_err(sd, "Wrong IOCTL cmd\n");
 +		break;
 +	}
 +
 +	return 0;
 +}
 +
 +static const struct v4l2_subdev_core_ops venc_core_ops = {
 +	.ioctl      = venc_ioctl,
 +};
 +
 +static const struct v4l2_subdev_video_ops venc_video_ops = {
 +	.s_routing = venc_s_routing,
 +	.s_std_output = venc_s_std_output,
 +	.s_dv_timings = venc_s_dv_timings,
 +};
 +
 +static const struct v4l2_subdev_ops venc_ops = {
 +	.core = &venc_core_ops,
 +	.video = &venc_video_ops,
 +};
 +
 +static int venc_initialize(struct v4l2_subdev *sd)
 +{
 +	struct venc_state *venc = to_state(sd);
 +	int ret;
 +
 +	/* Set default to output to composite and std to NTSC */
 +	venc->output = 0;
 +	venc->std = V4L2_STD_525_60;
 +
 +	ret = venc_s_routing(sd, 0, venc->output, 0);
 +	if (ret < 0) {
 +		v4l2_err(sd, "Error setting output during init\n");
 +		return -EINVAL;
 +	}
 +
 +	ret = venc_s_std_output(sd, venc->std);
 +	if (ret < 0) {
 +		v4l2_err(sd, "Error setting std during init\n");
 +		return -EINVAL;
 +	}
 +
 +	return ret;
 +}
 +
 +static int venc_device_get(struct device *dev, void *data)
 +{
 +	struct platform_device *pdev = to_platform_device(dev);
 +	struct venc_state **venc = data;
 +
 +	if (strcmp(MODULE_NAME, pdev->name) == 0)
 +		*venc = platform_get_drvdata(pdev);
 +
 +	return 0;
 +}
 +
 +struct v4l2_subdev *venc_sub_dev_init(struct v4l2_device *v4l2_dev,
 +		const char *venc_name)
 +{
 +	struct venc_state *venc;
 +	int err;
 +
 +	err = bus_for_each_dev(&platform_bus_type, NULL, &venc,
 +			venc_device_get);
 +	if (venc == NULL)
 +		return NULL;
 +
 +	v4l2_subdev_init(&venc->sd, &venc_ops);
 +
 +	strcpy(venc->sd.name, venc_name);
 +	if (v4l2_device_register_subdev(v4l2_dev, &venc->sd) < 0) {
 +		v4l2_err(v4l2_dev,
 +			"vpbe unable to register venc sub device\n");
 +		return NULL;
 +	}
 +	if (venc_initialize(&venc->sd)) {
 +		v4l2_err(v4l2_dev,
 +			"vpbe venc initialization failed\n");
 +		return NULL;
 +	}
 +
 +	return &venc->sd;
 +}
 +EXPORT_SYMBOL(venc_sub_dev_init);
 +
 +static int venc_probe(struct platform_device *pdev)
 +{
 +	struct venc_state *venc;
 +	struct resource *res;
 +	int ret;
 +
 +	venc = kzalloc(sizeof(struct venc_state), GFP_KERNEL);
 +	if (venc == NULL)
 +		return -ENOMEM;
 +
 +	venc->pdev = &pdev->dev;
 +	venc->pdata = pdev->dev.platform_data;
 +	if (NULL == venc->pdata) {
 +		dev_err(venc->pdev, "Unable to get platform data for"
 +			" VENC sub device");
 +		ret = -ENOENT;
 +		goto free_mem;
 +	}
 +	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 +	if (!res) {
 +		dev_err(venc->pdev,
 +			"Unable to get VENC register address map\n");
 +		ret = -ENODEV;
 +		goto free_mem;
 +	}
 +
 +	if (!request_mem_region(res->start, resource_size(res), "venc")) {
 +		dev_err(venc->pdev, "Unable to reserve VENC MMIO region\n");
 +		ret = -ENODEV;
 +		goto free_mem;
 +	}
 +
 +	venc->venc_base = ioremap_nocache(res->start, resource_size(res));
 +	if (!venc->venc_base) {
 +		dev_err(venc->pdev, "Unable to map VENC IO space\n");
 +		ret = -ENODEV;
 +		goto release_venc_mem_region;
 +	}
 +
 +	if (venc->pdata->venc_type != VPBE_VERSION_1) {
 +		res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
 +		if (!res) {
 +			dev_err(venc->pdev,
 +				"Unable to get VDAC_CONFIG address map\n");
 +			ret = -ENODEV;
 +			goto unmap_venc_io;
 +		}
 +
 +		if (!request_mem_region(res->start,
 +					resource_size(res), "venc")) {
 +			dev_err(venc->pdev,
 +				"Unable to reserve VDAC_CONFIG  MMIO region\n");
 +			ret = -ENODEV;
 +			goto unmap_venc_io;
 +		}
 +
 +		venc->vdaccfg_reg = ioremap_nocache(res->start,
 +						    resource_size(res));
 +		if (!venc->vdaccfg_reg) {
 +			dev_err(venc->pdev,
 +				"Unable to map VDAC_CONFIG IO space\n");
 +			ret = -ENODEV;
 +			goto release_vdaccfg_mem_region;
 +		}
 +	}
 +	spin_lock_init(&venc->lock);
 +	platform_set_drvdata(pdev, venc);
 +	dev_notice(venc->pdev, "VENC sub device probe success\n");
 +	return 0;
 +
 +release_vdaccfg_mem_region:
 +	release_mem_region(res->start, resource_size(res));
 +unmap_venc_io:
 +	iounmap(venc->venc_base);
 +release_venc_mem_region:
 +	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 +	release_mem_region(res->start, resource_size(res));
 +free_mem:
 +	kfree(venc);
 +	return ret;
 +}
 +
 +static int venc_remove(struct platform_device *pdev)
 +{
 +	struct venc_state *venc = platform_get_drvdata(pdev);
 +	struct resource *res;
 +
 +	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 +	iounmap((void *)venc->venc_base);
 +	release_mem_region(res->start, resource_size(res));
 +	if (venc->pdata->venc_type != VPBE_VERSION_1) {
 +		res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
 +		iounmap((void *)venc->vdaccfg_reg);
 +		release_mem_region(res->start, resource_size(res));
 +	}
 +	kfree(venc);
 +
 +	return 0;
 +}
 +
 +static struct platform_driver venc_driver = {
 +	.probe		= venc_probe,
 +	.remove		= venc_remove,
 +	.driver		= {
 +		.name	= MODULE_NAME,
 +		.owner	= THIS_MODULE,
 +	},
 +};
 +
 +module_platform_driver(venc_driver);
 +
 +MODULE_LICENSE("GPL");
 +MODULE_DESCRIPTION("VPBE VENC Driver");
 +MODULE_AUTHOR("Texas Instruments");
diff --cc drivers/media/platform/s5p-fimc/mipi-csis.c
index 3438ccf3c332,000000000000..22bdf211a2ac
mode 100644,000000..100644
--- a/drivers/media/platform/s5p-fimc/mipi-csis.c
+++ b/drivers/media/platform/s5p-fimc/mipi-csis.c
@@@ -1,888 -1,0 +1,888 @@@
 +/*
 + * Samsung S5P/EXYNOS4 SoC series MIPI-CSI receiver driver
 + *
 + * Copyright (C) 2011 - 2012 Samsung Electronics Co., Ltd.
 + * Sylwester Nawrocki <s.nawrocki@samsung.com>
 + *
 + * This program is free software; you can redistribute it and/or modify
 + * it under the terms of the GNU General Public License version 2 as
 + * published by the Free Software Foundation.
 + */
 +
 +#include <linux/clk.h>
 +#include <linux/delay.h>
 +#include <linux/device.h>
 +#include <linux/errno.h>
 +#include <linux/interrupt.h>
 +#include <linux/io.h>
 +#include <linux/irq.h>
 +#include <linux/kernel.h>
 +#include <linux/memory.h>
 +#include <linux/module.h>
 +#include <linux/platform_device.h>
 +#include <linux/pm_runtime.h>
 +#include <linux/regulator/consumer.h>
 +#include <linux/slab.h>
 +#include <linux/spinlock.h>
 +#include <linux/videodev2.h>
 +#include <media/v4l2-subdev.h>
- #include <plat/mipi_csis.h>
++#include <linux/platform_data/mipi-csis.h>
 +#include "mipi-csis.h"
 +
 +static int debug;
 +module_param(debug, int, 0644);
 +MODULE_PARM_DESC(debug, "Debug level (0-2)");
 +
 +/* Register map definition */
 +
 +/* CSIS global control */
 +#define S5PCSIS_CTRL			0x00
 +#define S5PCSIS_CTRL_DPDN_DEFAULT	(0 << 31)
 +#define S5PCSIS_CTRL_DPDN_SWAP		(1 << 31)
 +#define S5PCSIS_CTRL_ALIGN_32BIT	(1 << 20)
 +#define S5PCSIS_CTRL_UPDATE_SHADOW	(1 << 16)
 +#define S5PCSIS_CTRL_WCLK_EXTCLK	(1 << 8)
 +#define S5PCSIS_CTRL_RESET		(1 << 4)
 +#define S5PCSIS_CTRL_ENABLE		(1 << 0)
 +
 +/* D-PHY control */
 +#define S5PCSIS_DPHYCTRL		0x04
 +#define S5PCSIS_DPHYCTRL_HSS_MASK	(0x1f << 27)
 +#define S5PCSIS_DPHYCTRL_ENABLE		(0x1f << 0)
 +
 +#define S5PCSIS_CONFIG			0x08
 +#define S5PCSIS_CFG_FMT_YCBCR422_8BIT	(0x1e << 2)
 +#define S5PCSIS_CFG_FMT_RAW8		(0x2a << 2)
 +#define S5PCSIS_CFG_FMT_RAW10		(0x2b << 2)
 +#define S5PCSIS_CFG_FMT_RAW12		(0x2c << 2)
 +/* User defined formats, x = 1...4 */
 +#define S5PCSIS_CFG_FMT_USER(x)		((0x30 + x - 1) << 2)
 +#define S5PCSIS_CFG_FMT_MASK		(0x3f << 2)
 +#define S5PCSIS_CFG_NR_LANE_MASK	3
 +
 +/* Interrupt mask */
 +#define S5PCSIS_INTMSK			0x10
 +#define S5PCSIS_INTMSK_EN_ALL		0xf000103f
 +#define S5PCSIS_INTMSK_EVEN_BEFORE	(1 << 31)
 +#define S5PCSIS_INTMSK_EVEN_AFTER	(1 << 30)
 +#define S5PCSIS_INTMSK_ODD_BEFORE	(1 << 29)
 +#define S5PCSIS_INTMSK_ODD_AFTER	(1 << 28)
 +#define S5PCSIS_INTMSK_ERR_SOT_HS	(1 << 12)
 +#define S5PCSIS_INTMSK_ERR_LOST_FS	(1 << 5)
 +#define S5PCSIS_INTMSK_ERR_LOST_FE	(1 << 4)
 +#define S5PCSIS_INTMSK_ERR_OVER		(1 << 3)
 +#define S5PCSIS_INTMSK_ERR_ECC		(1 << 2)
 +#define S5PCSIS_INTMSK_ERR_CRC		(1 << 1)
 +#define S5PCSIS_INTMSK_ERR_UNKNOWN	(1 << 0)
 +
 +/* Interrupt source */
 +#define S5PCSIS_INTSRC			0x14
 +#define S5PCSIS_INTSRC_EVEN_BEFORE	(1 << 31)
 +#define S5PCSIS_INTSRC_EVEN_AFTER	(1 << 30)
 +#define S5PCSIS_INTSRC_EVEN		(0x3 << 30)
 +#define S5PCSIS_INTSRC_ODD_BEFORE	(1 << 29)
 +#define S5PCSIS_INTSRC_ODD_AFTER	(1 << 28)
 +#define S5PCSIS_INTSRC_ODD		(0x3 << 28)
 +#define S5PCSIS_INTSRC_NON_IMAGE_DATA	(0xff << 28)
 +#define S5PCSIS_INTSRC_ERR_SOT_HS	(0xf << 12)
 +#define S5PCSIS_INTSRC_ERR_LOST_FS	(1 << 5)
 +#define S5PCSIS_INTSRC_ERR_LOST_FE	(1 << 4)
 +#define S5PCSIS_INTSRC_ERR_OVER		(1 << 3)
 +#define S5PCSIS_INTSRC_ERR_ECC		(1 << 2)
 +#define S5PCSIS_INTSRC_ERR_CRC		(1 << 1)
 +#define S5PCSIS_INTSRC_ERR_UNKNOWN	(1 << 0)
 +#define S5PCSIS_INTSRC_ERRORS		0xf03f
 +
 +/* Pixel resolution */
 +#define S5PCSIS_RESOL			0x2c
 +#define CSIS_MAX_PIX_WIDTH		0xffff
 +#define CSIS_MAX_PIX_HEIGHT		0xffff
 +
 +/* Non-image packet data buffers */
 +#define S5PCSIS_PKTDATA_ODD		0x2000
 +#define S5PCSIS_PKTDATA_EVEN		0x3000
 +#define S5PCSIS_PKTDATA_SIZE		SZ_4K
 +
 +enum {
 +	CSIS_CLK_MUX,
 +	CSIS_CLK_GATE,
 +};
 +
 +static char *csi_clock_name[] = {
 +	[CSIS_CLK_MUX]  = "sclk_csis",
 +	[CSIS_CLK_GATE] = "csis",
 +};
 +#define NUM_CSIS_CLOCKS	ARRAY_SIZE(csi_clock_name)
 +
 +static const char * const csis_supply_name[] = {
 +	"vddcore",  /* CSIS Core (1.0V, 1.1V or 1.2V) suppply */
 +	"vddio",    /* CSIS I/O and PLL (1.8V) supply */
 +};
 +#define CSIS_NUM_SUPPLIES ARRAY_SIZE(csis_supply_name)
 +
 +enum {
 +	ST_POWERED	= 1,
 +	ST_STREAMING	= 2,
 +	ST_SUSPENDED	= 4,
 +};
 +
 +struct s5pcsis_event {
 +	u32 mask;
 +	const char * const name;
 +	unsigned int counter;
 +};
 +
 +static const struct s5pcsis_event s5pcsis_events[] = {
 +	/* Errors */
 +	{ S5PCSIS_INTSRC_ERR_SOT_HS,	"SOT Error" },
 +	{ S5PCSIS_INTSRC_ERR_LOST_FS,	"Lost Frame Start Error" },
 +	{ S5PCSIS_INTSRC_ERR_LOST_FE,	"Lost Frame End Error" },
 +	{ S5PCSIS_INTSRC_ERR_OVER,	"FIFO Overflow Error" },
 +	{ S5PCSIS_INTSRC_ERR_ECC,	"ECC Error" },
 +	{ S5PCSIS_INTSRC_ERR_CRC,	"CRC Error" },
 +	{ S5PCSIS_INTSRC_ERR_UNKNOWN,	"Unknown Error" },
 +	/* Non-image data receive events */
 +	{ S5PCSIS_INTSRC_EVEN_BEFORE,	"Non-image data before even frame" },
 +	{ S5PCSIS_INTSRC_EVEN_AFTER,	"Non-image data after even frame" },
 +	{ S5PCSIS_INTSRC_ODD_BEFORE,	"Non-image data before odd frame" },
 +	{ S5PCSIS_INTSRC_ODD_AFTER,	"Non-image data after odd frame" },
 +};
 +#define S5PCSIS_NUM_EVENTS ARRAY_SIZE(s5pcsis_events)
 +
 +struct csis_pktbuf {
 +	u32 *data;
 +	unsigned int len;
 +};
 +
 +/**
 + * struct csis_state - the driver's internal state data structure
 + * @lock: mutex serializing the subdev and power management operations,
 + *        protecting @format and @flags members
 + * @pads: CSIS pads array
 + * @sd: v4l2_subdev associated with CSIS device instance
 + * @pdev: CSIS platform device
 + * @regs: mmaped I/O registers memory
 + * @supplies: CSIS regulator supplies
 + * @clock: CSIS clocks
 + * @irq: requested s5p-mipi-csis irq number
 + * @flags: the state variable for power and streaming control
 + * @csis_fmt: current CSIS pixel format
 + * @format: common media bus format for the source and sink pad
 + * @slock: spinlock protecting structure members below
 + * @pkt_buf: the frame embedded (non-image) data buffer
 + * @events: MIPI-CSIS event (error) counters
 + */
 +struct csis_state {
 +	struct mutex lock;
 +	struct media_pad pads[CSIS_PADS_NUM];
 +	struct v4l2_subdev sd;
 +	struct platform_device *pdev;
 +	void __iomem *regs;
 +	struct regulator_bulk_data supplies[CSIS_NUM_SUPPLIES];
 +	struct clk *clock[NUM_CSIS_CLOCKS];
 +	int irq;
 +	u32 flags;
 +	const struct csis_pix_format *csis_fmt;
 +	struct v4l2_mbus_framefmt format;
 +
 +	struct spinlock slock;
 +	struct csis_pktbuf pkt_buf;
 +	struct s5pcsis_event events[S5PCSIS_NUM_EVENTS];
 +};
 +
 +/**
 + * struct csis_pix_format - CSIS pixel format description
 + * @pix_width_alignment: horizontal pixel alignment, width will be
 + *                       multiple of 2^pix_width_alignment
 + * @code: corresponding media bus code
 + * @fmt_reg: S5PCSIS_CONFIG register value
 + * @data_alignment: MIPI-CSI data alignment in bits
 + */
 +struct csis_pix_format {
 +	unsigned int pix_width_alignment;
 +	enum v4l2_mbus_pixelcode code;
 +	u32 fmt_reg;
 +	u8 data_alignment;
 +};
 +
 +static const struct csis_pix_format s5pcsis_formats[] = {
 +	{
 +		.code = V4L2_MBUS_FMT_VYUY8_2X8,
 +		.fmt_reg = S5PCSIS_CFG_FMT_YCBCR422_8BIT,
 +		.data_alignment = 32,
 +	}, {
 +		.code = V4L2_MBUS_FMT_JPEG_1X8,
 +		.fmt_reg = S5PCSIS_CFG_FMT_USER(1),
 +		.data_alignment = 32,
 +	}, {
 +		.code = V4L2_MBUS_FMT_S5C_UYVY_JPEG_1X8,
 +		.fmt_reg = S5PCSIS_CFG_FMT_USER(1),
 +		.data_alignment = 32,
 +	}
 +};
 +
 +#define s5pcsis_write(__csis, __r, __v) writel(__v, __csis->regs + __r)
 +#define s5pcsis_read(__csis, __r) readl(__csis->regs + __r)
 +
 +static struct csis_state *sd_to_csis_state(struct v4l2_subdev *sdev)
 +{
 +	return container_of(sdev, struct csis_state, sd);
 +}
 +
 +static const struct csis_pix_format *find_csis_format(
 +	struct v4l2_mbus_framefmt *mf)
 +{
 +	int i;
 +
 +	for (i = 0; i < ARRAY_SIZE(s5pcsis_formats); i++)
 +		if (mf->code == s5pcsis_formats[i].code)
 +			return &s5pcsis_formats[i];
 +	return NULL;
 +}
 +
 +static void s5pcsis_enable_interrupts(struct csis_state *state, bool on)
 +{
 +	u32 val = s5pcsis_read(state, S5PCSIS_INTMSK);
 +
 +	val = on ? val | S5PCSIS_INTMSK_EN_ALL :
 +		   val & ~S5PCSIS_INTMSK_EN_ALL;
 +	s5pcsis_write(state, S5PCSIS_INTMSK, val);
 +}
 +
 +static void s5pcsis_reset(struct csis_state *state)
 +{
 +	u32 val = s5pcsis_read(state, S5PCSIS_CTRL);
 +
 +	s5pcsis_write(state, S5PCSIS_CTRL, val | S5PCSIS_CTRL_RESET);
 +	udelay(10);
 +}
 +
 +static void s5pcsis_system_enable(struct csis_state *state, int on)
 +{
 +	u32 val;
 +
 +	val = s5pcsis_read(state, S5PCSIS_CTRL);
 +	if (on)
 +		val |= S5PCSIS_CTRL_ENABLE;
 +	else
 +		val &= ~S5PCSIS_CTRL_ENABLE;
 +	s5pcsis_write(state, S5PCSIS_CTRL, val);
 +
 +	val = s5pcsis_read(state, S5PCSIS_DPHYCTRL);
 +	if (on)
 +		val |= S5PCSIS_DPHYCTRL_ENABLE;
 +	else
 +		val &= ~S5PCSIS_DPHYCTRL_ENABLE;
 +	s5pcsis_write(state, S5PCSIS_DPHYCTRL, val);
 +}
 +
 +/* Called with the state.lock mutex held */
 +static void __s5pcsis_set_format(struct csis_state *state)
 +{
 +	struct v4l2_mbus_framefmt *mf = &state->format;
 +	u32 val;
 +
 +	v4l2_dbg(1, debug, &state->sd, "fmt: %#x, %d x %d\n",
 +		 mf->code, mf->width, mf->height);
 +
 +	/* Color format */
 +	val = s5pcsis_read(state, S5PCSIS_CONFIG);
 +	val = (val & ~S5PCSIS_CFG_FMT_MASK) | state->csis_fmt->fmt_reg;
 +	s5pcsis_write(state, S5PCSIS_CONFIG, val);
 +
 +	/* Pixel resolution */
 +	val = (mf->width << 16) | mf->height;
 +	s5pcsis_write(state, S5PCSIS_RESOL, val);
 +}
 +
 +static void s5pcsis_set_hsync_settle(struct csis_state *state, int settle)
 +{
 +	u32 val = s5pcsis_read(state, S5PCSIS_DPHYCTRL);
 +
 +	val = (val & ~S5PCSIS_DPHYCTRL_HSS_MASK) | (settle << 27);
 +	s5pcsis_write(state, S5PCSIS_DPHYCTRL, val);
 +}
 +
 +static void s5pcsis_set_params(struct csis_state *state)
 +{
 +	struct s5p_platform_mipi_csis *pdata = state->pdev->dev.platform_data;
 +	u32 val;
 +
 +	val = s5pcsis_read(state, S5PCSIS_CONFIG);
 +	val = (val & ~S5PCSIS_CFG_NR_LANE_MASK) | (pdata->lanes - 1);
 +	s5pcsis_write(state, S5PCSIS_CONFIG, val);
 +
 +	__s5pcsis_set_format(state);
 +	s5pcsis_set_hsync_settle(state, pdata->hs_settle);
 +
 +	val = s5pcsis_read(state, S5PCSIS_CTRL);
 +	if (state->csis_fmt->data_alignment == 32)
 +		val |= S5PCSIS_CTRL_ALIGN_32BIT;
 +	else /* 24-bits */
 +		val &= ~S5PCSIS_CTRL_ALIGN_32BIT;
 +	/* Not using external clock. */
 +	val &= ~S5PCSIS_CTRL_WCLK_EXTCLK;
 +	s5pcsis_write(state, S5PCSIS_CTRL, val);
 +
 +	/* Update the shadow register. */
 +	val = s5pcsis_read(state, S5PCSIS_CTRL);
 +	s5pcsis_write(state, S5PCSIS_CTRL, val | S5PCSIS_CTRL_UPDATE_SHADOW);
 +}
 +
 +static void s5pcsis_clk_put(struct csis_state *state)
 +{
 +	int i;
 +
 +	for (i = 0; i < NUM_CSIS_CLOCKS; i++) {
 +		if (IS_ERR_OR_NULL(state->clock[i]))
 +			continue;
 +		clk_unprepare(state->clock[i]);
 +		clk_put(state->clock[i]);
 +		state->clock[i] = NULL;
 +	}
 +}
 +
 +static int s5pcsis_clk_get(struct csis_state *state)
 +{
 +	struct device *dev = &state->pdev->dev;
 +	int i, ret;
 +
 +	for (i = 0; i < NUM_CSIS_CLOCKS; i++) {
 +		state->clock[i] = clk_get(dev, csi_clock_name[i]);
 +		if (IS_ERR(state->clock[i]))
 +			goto err;
 +		ret = clk_prepare(state->clock[i]);
 +		if (ret < 0) {
 +			clk_put(state->clock[i]);
 +			state->clock[i] = NULL;
 +			goto err;
 +		}
 +	}
 +	return 0;
 +err:
 +	s5pcsis_clk_put(state);
 +	dev_err(dev, "failed to get clock: %s\n", csi_clock_name[i]);
 +	return -ENXIO;
 +}
 +
 +static void s5pcsis_start_stream(struct csis_state *state)
 +{
 +	s5pcsis_reset(state);
 +	s5pcsis_set_params(state);
 +	s5pcsis_system_enable(state, true);
 +	s5pcsis_enable_interrupts(state, true);
 +}
 +
 +static void s5pcsis_stop_stream(struct csis_state *state)
 +{
 +	s5pcsis_enable_interrupts(state, false);
 +	s5pcsis_system_enable(state, false);
 +}
 +
 +static void s5pcsis_clear_counters(struct csis_state *state)
 +{
 +	unsigned long flags;
 +	int i;
 +
 +	spin_lock_irqsave(&state->slock, flags);
 +	for (i = 0; i < S5PCSIS_NUM_EVENTS; i++)
 +		state->events[i].counter = 0;
 +	spin_unlock_irqrestore(&state->slock, flags);
 +}
 +
 +static void s5pcsis_log_counters(struct csis_state *state, bool non_errors)
 +{
 +	int i = non_errors ? S5PCSIS_NUM_EVENTS : S5PCSIS_NUM_EVENTS - 4;
 +	unsigned long flags;
 +
 +	spin_lock_irqsave(&state->slock, flags);
 +
 +	for (i--; i >= 0; i--)
 +		if (state->events[i].counter >= 0)
 +			v4l2_info(&state->sd, "%s events: %d\n",
 +				  state->events[i].name,
 +				  state->events[i].counter);
 +
 +	spin_unlock_irqrestore(&state->slock, flags);
 +}
 +
 +/*
 + * V4L2 subdev operations
 + */
 +static int s5pcsis_s_power(struct v4l2_subdev *sd, int on)
 +{
 +	struct csis_state *state = sd_to_csis_state(sd);
 +	struct device *dev = &state->pdev->dev;
 +
 +	if (on)
 +		return pm_runtime_get_sync(dev);
 +
 +	return pm_runtime_put_sync(dev);
 +}
 +
 +static int s5pcsis_s_stream(struct v4l2_subdev *sd, int enable)
 +{
 +	struct csis_state *state = sd_to_csis_state(sd);
 +	int ret = 0;
 +
 +	v4l2_dbg(1, debug, sd, "%s: %d, state: 0x%x\n",
 +		 __func__, enable, state->flags);
 +
 +	if (enable) {
 +		s5pcsis_clear_counters(state);
 +		ret = pm_runtime_get_sync(&state->pdev->dev);
 +		if (ret && ret != 1)
 +			return ret;
 +	}
 +
 +	mutex_lock(&state->lock);
 +	if (enable) {
 +		if (state->flags & ST_SUSPENDED) {
 +			ret = -EBUSY;
 +			goto unlock;
 +		}
 +		s5pcsis_start_stream(state);
 +		state->flags |= ST_STREAMING;
 +	} else {
 +		s5pcsis_stop_stream(state);
 +		state->flags &= ~ST_STREAMING;
 +		if (debug > 0)
 +			s5pcsis_log_counters(state, true);
 +	}
 +unlock:
 +	mutex_unlock(&state->lock);
 +	if (!enable)
 +		pm_runtime_put(&state->pdev->dev);
 +
 +	return ret == 1 ? 0 : ret;
 +}
 +
 +static int s5pcsis_enum_mbus_code(struct v4l2_subdev *sd,
 +				  struct v4l2_subdev_fh *fh,
 +				  struct v4l2_subdev_mbus_code_enum *code)
 +{
 +	if (code->index >= ARRAY_SIZE(s5pcsis_formats))
 +		return -EINVAL;
 +
 +	code->code = s5pcsis_formats[code->index].code;
 +	return 0;
 +}
 +
 +static struct csis_pix_format const *s5pcsis_try_format(
 +	struct v4l2_mbus_framefmt *mf)
 +{
 +	struct csis_pix_format const *csis_fmt;
 +
 +	csis_fmt = find_csis_format(mf);
 +	if (csis_fmt == NULL)
 +		csis_fmt = &s5pcsis_formats[0];
 +
 +	mf->code = csis_fmt->code;
 +	v4l_bound_align_image(&mf->width, 1, CSIS_MAX_PIX_WIDTH,
 +			      csis_fmt->pix_width_alignment,
 +			      &mf->height, 1, CSIS_MAX_PIX_HEIGHT, 1,
 +			      0);
 +	return csis_fmt;
 +}
 +
 +static struct v4l2_mbus_framefmt *__s5pcsis_get_format(
 +		struct csis_state *state, struct v4l2_subdev_fh *fh,
 +		u32 pad, enum v4l2_subdev_format_whence which)
 +{
 +	if (which == V4L2_SUBDEV_FORMAT_TRY)
 +		return fh ? v4l2_subdev_get_try_format(fh, pad) : NULL;
 +
 +	return &state->format;
 +}
 +
 +static int s5pcsis_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 +			   struct v4l2_subdev_format *fmt)
 +{
 +	struct csis_state *state = sd_to_csis_state(sd);
 +	struct csis_pix_format const *csis_fmt;
 +	struct v4l2_mbus_framefmt *mf;
 +
 +	if (fmt->pad != CSIS_PAD_SOURCE && fmt->pad != CSIS_PAD_SINK)
 +		return -EINVAL;
 +
 +	mf = __s5pcsis_get_format(state, fh, fmt->pad, fmt->which);
 +
 +	if (fmt->pad == CSIS_PAD_SOURCE) {
 +		if (mf) {
 +			mutex_lock(&state->lock);
 +			fmt->format = *mf;
 +			mutex_unlock(&state->lock);
 +		}
 +		return 0;
 +	}
 +	csis_fmt = s5pcsis_try_format(&fmt->format);
 +	if (mf) {
 +		mutex_lock(&state->lock);
 +		*mf = fmt->format;
 +		if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
 +			state->csis_fmt = csis_fmt;
 +		mutex_unlock(&state->lock);
 +	}
 +	return 0;
 +}
 +
 +static int s5pcsis_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 +			   struct v4l2_subdev_format *fmt)
 +{
 +	struct csis_state *state = sd_to_csis_state(sd);
 +	struct v4l2_mbus_framefmt *mf;
 +
 +	if (fmt->pad != CSIS_PAD_SOURCE && fmt->pad != CSIS_PAD_SINK)
 +		return -EINVAL;
 +
 +	mf = __s5pcsis_get_format(state, fh, fmt->pad, fmt->which);
 +	if (!mf)
 +		return -EINVAL;
 +
 +	mutex_lock(&state->lock);
 +	fmt->format = *mf;
 +	mutex_unlock(&state->lock);
 +	return 0;
 +}
 +
 +static int s5pcsis_s_rx_buffer(struct v4l2_subdev *sd, void *buf,
 +			       unsigned int *size)
 +{
 +	struct csis_state *state = sd_to_csis_state(sd);
 +	unsigned long flags;
 +
 +	*size = min_t(unsigned int, *size, S5PCSIS_PKTDATA_SIZE);
 +
 +	spin_lock_irqsave(&state->slock, flags);
 +	state->pkt_buf.data = buf;
 +	state->pkt_buf.len = *size;
 +	spin_unlock_irqrestore(&state->slock, flags);
 +
 +	return 0;
 +}
 +
 +static int s5pcsis_log_status(struct v4l2_subdev *sd)
 +{
 +	struct csis_state *state = sd_to_csis_state(sd);
 +
 +	s5pcsis_log_counters(state, true);
 +	return 0;
 +}
 +
 +static int s5pcsis_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 +{
 +	struct v4l2_mbus_framefmt *format = v4l2_subdev_get_try_format(fh, 0);
 +
 +	format->colorspace = V4L2_COLORSPACE_JPEG;
 +	format->code = s5pcsis_formats[0].code;
 +	format->width = S5PCSIS_DEF_PIX_WIDTH;
 +	format->height = S5PCSIS_DEF_PIX_HEIGHT;
 +	format->field = V4L2_FIELD_NONE;
 +
 +	return 0;
 +}
 +
 +static const struct v4l2_subdev_internal_ops s5pcsis_sd_internal_ops = {
 +	.open = s5pcsis_open,
 +};
 +
 +static struct v4l2_subdev_core_ops s5pcsis_core_ops = {
 +	.s_power = s5pcsis_s_power,
 +	.log_status = s5pcsis_log_status,
 +};
 +
 +static struct v4l2_subdev_pad_ops s5pcsis_pad_ops = {
 +	.enum_mbus_code = s5pcsis_enum_mbus_code,
 +	.get_fmt = s5pcsis_get_fmt,
 +	.set_fmt = s5pcsis_set_fmt,
 +};
 +
 +static struct v4l2_subdev_video_ops s5pcsis_video_ops = {
 +	.s_rx_buffer = s5pcsis_s_rx_buffer,
 +	.s_stream = s5pcsis_s_stream,
 +};
 +
 +static struct v4l2_subdev_ops s5pcsis_subdev_ops = {
 +	.core = &s5pcsis_core_ops,
 +	.pad = &s5pcsis_pad_ops,
 +	.video = &s5pcsis_video_ops,
 +};
 +
 +static irqreturn_t s5pcsis_irq_handler(int irq, void *dev_id)
 +{
 +	struct csis_state *state = dev_id;
 +	struct csis_pktbuf *pktbuf = &state->pkt_buf;
 +	unsigned long flags;
 +	u32 status;
 +
 +	status = s5pcsis_read(state, S5PCSIS_INTSRC);
 +	spin_lock_irqsave(&state->slock, flags);
 +
 +	if ((status & S5PCSIS_INTSRC_NON_IMAGE_DATA) && pktbuf->data) {
 +		u32 offset;
 +
 +		if (status & S5PCSIS_INTSRC_EVEN)
 +			offset = S5PCSIS_PKTDATA_EVEN;
 +		else
 +			offset = S5PCSIS_PKTDATA_ODD;
 +
 +		memcpy(pktbuf->data, state->regs + offset, pktbuf->len);
 +		pktbuf->data = NULL;
 +		rmb();
 +	}
 +
 +	/* Update the event/error counters */
 +	if ((status & S5PCSIS_INTSRC_ERRORS) || debug) {
 +		int i;
 +		for (i = 0; i < S5PCSIS_NUM_EVENTS; i++) {
 +			if (!(status & state->events[i].mask))
 +				continue;
 +			state->events[i].counter++;
 +			v4l2_dbg(2, debug, &state->sd, "%s: %d\n",
 +				 state->events[i].name,
 +				 state->events[i].counter);
 +		}
 +		v4l2_dbg(2, debug, &state->sd, "status: %08x\n", status);
 +	}
 +	spin_unlock_irqrestore(&state->slock, flags);
 +
 +	s5pcsis_write(state, S5PCSIS_INTSRC, status);
 +	return IRQ_HANDLED;
 +}
 +
 +static int __devinit s5pcsis_probe(struct platform_device *pdev)
 +{
 +	struct s5p_platform_mipi_csis *pdata;
 +	struct resource *mem_res;
 +	struct csis_state *state;
 +	int ret = -ENOMEM;
 +	int i;
 +
 +	state = devm_kzalloc(&pdev->dev, sizeof(*state), GFP_KERNEL);
 +	if (!state)
 +		return -ENOMEM;
 +
 +	mutex_init(&state->lock);
 +	spin_lock_init(&state->slock);
 +
 +	state->pdev = pdev;
 +
 +	pdata = pdev->dev.platform_data;
 +	if (pdata == NULL || pdata->phy_enable == NULL) {
 +		dev_err(&pdev->dev, "Platform data not fully specified\n");
 +		return -EINVAL;
 +	}
 +
 +	if ((pdev->id == 1 && pdata->lanes > CSIS1_MAX_LANES) ||
 +	    pdata->lanes > CSIS0_MAX_LANES) {
 +		dev_err(&pdev->dev, "Unsupported number of data lanes: %d\n",
 +			pdata->lanes);
 +		return -EINVAL;
 +	}
 +
 +	mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 +	state->regs = devm_request_and_ioremap(&pdev->dev, mem_res);
 +	if (state->regs == NULL) {
 +		dev_err(&pdev->dev, "Failed to request and remap io memory\n");
 +		return -ENXIO;
 +	}
 +
 +	state->irq = platform_get_irq(pdev, 0);
 +	if (state->irq < 0) {
 +		dev_err(&pdev->dev, "Failed to get irq\n");
 +		return state->irq;
 +	}
 +
 +	for (i = 0; i < CSIS_NUM_SUPPLIES; i++)
 +		state->supplies[i].supply = csis_supply_name[i];
 +
 +	ret = regulator_bulk_get(&pdev->dev, CSIS_NUM_SUPPLIES,
 +				 state->supplies);
 +	if (ret)
 +		return ret;
 +
 +	ret = s5pcsis_clk_get(state);
 +	if (ret)
 +		goto e_clkput;
 +
 +	clk_enable(state->clock[CSIS_CLK_MUX]);
 +	if (pdata->clk_rate)
 +		clk_set_rate(state->clock[CSIS_CLK_MUX], pdata->clk_rate);
 +	else
 +		dev_WARN(&pdev->dev, "No clock frequency specified!\n");
 +
 +	ret = devm_request_irq(&pdev->dev, state->irq, s5pcsis_irq_handler,
 +			       0, dev_name(&pdev->dev), state);
 +	if (ret) {
 +		dev_err(&pdev->dev, "Interrupt request failed\n");
 +		goto e_regput;
 +	}
 +
 +	v4l2_subdev_init(&state->sd, &s5pcsis_subdev_ops);
 +	state->sd.owner = THIS_MODULE;
 +	strlcpy(state->sd.name, dev_name(&pdev->dev), sizeof(state->sd.name));
 +	state->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 +	state->csis_fmt = &s5pcsis_formats[0];
 +
 +	state->format.code = s5pcsis_formats[0].code;
 +	state->format.width = S5PCSIS_DEF_PIX_WIDTH;
 +	state->format.height = S5PCSIS_DEF_PIX_HEIGHT;
 +
 +	state->pads[CSIS_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
 +	state->pads[CSIS_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
 +	ret = media_entity_init(&state->sd.entity,
 +				CSIS_PADS_NUM, state->pads, 0);
 +	if (ret < 0)
 +		goto e_clkput;
 +
 +	/* This allows to retrieve the platform device id by the host driver */
 +	v4l2_set_subdevdata(&state->sd, pdev);
 +
 +	/* .. and a pointer to the subdev. */
 +	platform_set_drvdata(pdev, &state->sd);
 +
 +	memcpy(state->events, s5pcsis_events, sizeof(state->events));
 +
 +	pm_runtime_enable(&pdev->dev);
 +	return 0;
 +
 +e_regput:
 +	regulator_bulk_free(CSIS_NUM_SUPPLIES, state->supplies);
 +e_clkput:
 +	clk_disable(state->clock[CSIS_CLK_MUX]);
 +	s5pcsis_clk_put(state);
 +	return ret;
 +}
 +
 +static int s5pcsis_pm_suspend(struct device *dev, bool runtime)
 +{
 +	struct s5p_platform_mipi_csis *pdata = dev->platform_data;
 +	struct platform_device *pdev = to_platform_device(dev);
 +	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
 +	struct csis_state *state = sd_to_csis_state(sd);
 +	int ret = 0;
 +
 +	v4l2_dbg(1, debug, sd, "%s: flags: 0x%x\n",
 +		 __func__, state->flags);
 +
 +	mutex_lock(&state->lock);
 +	if (state->flags & ST_POWERED) {
 +		s5pcsis_stop_stream(state);
 +		ret = pdata->phy_enable(state->pdev, false);
 +		if (ret)
 +			goto unlock;
 +		ret = regulator_bulk_disable(CSIS_NUM_SUPPLIES,
 +					     state->supplies);
 +		if (ret)
 +			goto unlock;
 +		clk_disable(state->clock[CSIS_CLK_GATE]);
 +		state->flags &= ~ST_POWERED;
 +		if (!runtime)
 +			state->flags |= ST_SUSPENDED;
 +	}
 + unlock:
 +	mutex_unlock(&state->lock);
 +	return ret ? -EAGAIN : 0;
 +}
 +
 +static int s5pcsis_pm_resume(struct device *dev, bool runtime)
 +{
 +	struct s5p_platform_mipi_csis *pdata = dev->platform_data;
 +	struct platform_device *pdev = to_platform_device(dev);
 +	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
 +	struct csis_state *state = sd_to_csis_state(sd);
 +	int ret = 0;
 +
 +	v4l2_dbg(1, debug, sd, "%s: flags: 0x%x\n",
 +		 __func__, state->flags);
 +
 +	mutex_lock(&state->lock);
 +	if (!runtime && !(state->flags & ST_SUSPENDED))
 +		goto unlock;
 +
 +	if (!(state->flags & ST_POWERED)) {
 +		ret = regulator_bulk_enable(CSIS_NUM_SUPPLIES,
 +					    state->supplies);
 +		if (ret)
 +			goto unlock;
 +		ret = pdata->phy_enable(state->pdev, true);
 +		if (!ret) {
 +			state->flags |= ST_POWERED;
 +		} else {
 +			regulator_bulk_disable(CSIS_NUM_SUPPLIES,
 +					       state->supplies);
 +			goto unlock;
 +		}
 +		clk_enable(state->clock[CSIS_CLK_GATE]);
 +	}
 +	if (state->flags & ST_STREAMING)
 +		s5pcsis_start_stream(state);
 +
 +	state->flags &= ~ST_SUSPENDED;
 + unlock:
 +	mutex_unlock(&state->lock);
 +	return ret ? -EAGAIN : 0;
 +}
 +
 +#ifdef CONFIG_PM_SLEEP
 +static int s5pcsis_suspend(struct device *dev)
 +{
 +	return s5pcsis_pm_suspend(dev, false);
 +}
 +
 +static int s5pcsis_resume(struct device *dev)
 +{
 +	return s5pcsis_pm_resume(dev, false);
 +}
 +#endif
 +
 +#ifdef CONFIG_PM_RUNTIME
 +static int s5pcsis_runtime_suspend(struct device *dev)
 +{
 +	return s5pcsis_pm_suspend(dev, true);
 +}
 +
 +static int s5pcsis_runtime_resume(struct device *dev)
 +{
 +	return s5pcsis_pm_resume(dev, true);
 +}
 +#endif
 +
 +static int __devexit s5pcsis_remove(struct platform_device *pdev)
 +{
 +	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
 +	struct csis_state *state = sd_to_csis_state(sd);
 +
 +	pm_runtime_disable(&pdev->dev);
 +	s5pcsis_pm_suspend(&pdev->dev, false);
 +	clk_disable(state->clock[CSIS_CLK_MUX]);
 +	pm_runtime_set_suspended(&pdev->dev);
 +	s5pcsis_clk_put(state);
 +	regulator_bulk_free(CSIS_NUM_SUPPLIES, state->supplies);
 +
 +	media_entity_cleanup(&state->sd.entity);
 +
 +	return 0;
 +}
 +
 +static const struct dev_pm_ops s5pcsis_pm_ops = {
 +	SET_RUNTIME_PM_OPS(s5pcsis_runtime_suspend, s5pcsis_runtime_resume,
 +			   NULL)
 +	SET_SYSTEM_SLEEP_PM_OPS(s5pcsis_suspend, s5pcsis_resume)
 +};
 +
 +static struct platform_driver s5pcsis_driver = {
 +	.probe		= s5pcsis_probe,
 +	.remove		= __devexit_p(s5pcsis_remove),
 +	.driver		= {
 +		.name	= CSIS_DRIVER_NAME,
 +		.owner	= THIS_MODULE,
 +		.pm	= &s5pcsis_pm_ops,
 +	},
 +};
 +
 +module_platform_driver(s5pcsis_driver);
 +
 +MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
 +MODULE_DESCRIPTION("Samsung S5P/EXYNOS SoC MIPI-CSI2 receiver driver");
 +MODULE_LICENSE("GPL");
diff --cc drivers/media/platform/soc_camera/mx1_camera.c
index 560a65aa7038,000000000000..bbe70991d30b
mode 100644,000000..100644
--- a/drivers/media/platform/soc_camera/mx1_camera.c
+++ b/drivers/media/platform/soc_camera/mx1_camera.c
@@@ -1,889 -1,0 +1,889 @@@
 +/*
 + * V4L2 Driver for i.MXL/i.MXL camera (CSI) host
 + *
 + * Copyright (C) 2008, Paulius Zaleckas <paulius.zaleckas@teltonika.lt>
 + * Copyright (C) 2009, Darius Augulis <augulis.darius@gmail.com>
 + *
 + * Based on PXA SoC camera driver
 + * Copyright (C) 2006, Sascha Hauer, Pengutronix
 + * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
 + *
 + * This program is free software; you can redistribute it and/or modify
 + * it under the terms of the GNU General Public License version 2 as
 + * published by the Free Software Foundation.
 + */
 +
 +#include <linux/clk.h>
 +#include <linux/delay.h>
 +#include <linux/device.h>
 +#include <linux/dma-mapping.h>
 +#include <linux/errno.h>
 +#include <linux/fs.h>
 +#include <linux/init.h>
 +#include <linux/interrupt.h>
 +#include <linux/io.h>
 +#include <linux/kernel.h>
 +#include <linux/mm.h>
 +#include <linux/module.h>
 +#include <linux/moduleparam.h>
 +#include <linux/mutex.h>
 +#include <linux/platform_device.h>
 +#include <linux/sched.h>
 +#include <linux/slab.h>
 +#include <linux/time.h>
 +#include <linux/videodev2.h>
 +
 +#include <media/soc_camera.h>
 +#include <media/v4l2-common.h>
 +#include <media/v4l2-dev.h>
 +#include <media/videobuf-dma-contig.h>
 +#include <media/soc_mediabus.h>
 +
 +#include <asm/dma.h>
 +#include <asm/fiq.h>
 +#include <mach/dma-mx1-mx2.h>
 +#include <mach/hardware.h>
 +#include <mach/irqs.h>
- #include <mach/mx1_camera.h>
++#include <linux/platform_data/camera-mx1.h>
 +
 +/*
 + * CSI registers
 + */
 +#define CSICR1		0x00			/* CSI Control Register 1 */
 +#define CSISR		0x08			/* CSI Status Register */
 +#define CSIRXR		0x10			/* CSI RxFIFO Register */
 +
 +#define CSICR1_RXFF_LEVEL(x)	(((x) & 0x3) << 19)
 +#define CSICR1_SOF_POL		(1 << 17)
 +#define CSICR1_SOF_INTEN	(1 << 16)
 +#define CSICR1_MCLKDIV(x)	(((x) & 0xf) << 12)
 +#define CSICR1_MCLKEN		(1 << 9)
 +#define CSICR1_FCC		(1 << 8)
 +#define CSICR1_BIG_ENDIAN	(1 << 7)
 +#define CSICR1_CLR_RXFIFO	(1 << 5)
 +#define CSICR1_GCLK_MODE	(1 << 4)
 +#define CSICR1_DATA_POL		(1 << 2)
 +#define CSICR1_REDGE		(1 << 1)
 +#define CSICR1_EN		(1 << 0)
 +
 +#define CSISR_SFF_OR_INT	(1 << 25)
 +#define CSISR_RFF_OR_INT	(1 << 24)
 +#define CSISR_STATFF_INT	(1 << 21)
 +#define CSISR_RXFF_INT		(1 << 18)
 +#define CSISR_SOF_INT		(1 << 16)
 +#define CSISR_DRDY		(1 << 0)
 +
 +#define DRIVER_VERSION "0.0.2"
 +#define DRIVER_NAME "mx1-camera"
 +
 +#define CSI_IRQ_MASK	(CSISR_SFF_OR_INT | CSISR_RFF_OR_INT | \
 +			CSISR_STATFF_INT | CSISR_RXFF_INT | CSISR_SOF_INT)
 +
 +#define CSI_BUS_FLAGS	(V4L2_MBUS_MASTER | V4L2_MBUS_HSYNC_ACTIVE_HIGH | \
 +			V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_LOW | \
 +			V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING | \
 +			V4L2_MBUS_DATA_ACTIVE_HIGH | V4L2_MBUS_DATA_ACTIVE_LOW)
 +
 +#define MAX_VIDEO_MEM 16	/* Video memory limit in megabytes */
 +
 +/*
 + * Structures
 + */
 +
 +/* buffer for one video frame */
 +struct mx1_buffer {
 +	/* common v4l buffer stuff -- must be first */
 +	struct videobuf_buffer		vb;
 +	enum v4l2_mbus_pixelcode	code;
 +	int				inwork;
 +};
 +
 +/*
 + * i.MX1/i.MXL is only supposed to handle one camera on its Camera Sensor
 + * Interface. If anyone ever builds hardware to enable more than
 + * one camera, they will have to modify this driver too
 + */
 +struct mx1_camera_dev {
 +	struct soc_camera_host		soc_host;
 +	struct soc_camera_device	*icd;
 +	struct mx1_camera_pdata		*pdata;
 +	struct mx1_buffer		*active;
 +	struct resource			*res;
 +	struct clk			*clk;
 +	struct list_head		capture;
 +
 +	void __iomem			*base;
 +	int				dma_chan;
 +	unsigned int			irq;
 +	unsigned long			mclk;
 +
 +	spinlock_t			lock;
 +};
 +
 +/*
 + *  Videobuf operations
 + */
 +static int mx1_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 +			      unsigned int *size)
 +{
 +	struct soc_camera_device *icd = vq->priv_data;
 +
 +	*size = icd->sizeimage;
 +
 +	if (!*count)
 +		*count = 32;
 +
 +	if (*size * *count > MAX_VIDEO_MEM * 1024 * 1024)
 +		*count = (MAX_VIDEO_MEM * 1024 * 1024) / *size;
 +
 +	dev_dbg(icd->parent, "count=%d, size=%d\n", *count, *size);
 +
 +	return 0;
 +}
 +
 +static void free_buffer(struct videobuf_queue *vq, struct mx1_buffer *buf)
 +{
 +	struct soc_camera_device *icd = vq->priv_data;
 +	struct videobuf_buffer *vb = &buf->vb;
 +
 +	BUG_ON(in_interrupt());
 +
 +	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 +		vb, vb->baddr, vb->bsize);
 +
 +	/*
 +	 * This waits until this buffer is out of danger, i.e., until it is no
 +	 * longer in STATE_QUEUED or STATE_ACTIVE
 +	 */
 +	videobuf_waiton(vq, vb, 0, 0);
 +	videobuf_dma_contig_free(vq, vb);
 +
 +	vb->state = VIDEOBUF_NEEDS_INIT;
 +}
 +
 +static int mx1_videobuf_prepare(struct videobuf_queue *vq,
 +		struct videobuf_buffer *vb, enum v4l2_field field)
 +{
 +	struct soc_camera_device *icd = vq->priv_data;
 +	struct mx1_buffer *buf = container_of(vb, struct mx1_buffer, vb);
 +	int ret;
 +
 +	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 +		vb, vb->baddr, vb->bsize);
 +
 +	/* Added list head initialization on alloc */
 +	WARN_ON(!list_empty(&vb->queue));
 +
 +	BUG_ON(NULL == icd->current_fmt);
 +
 +	/*
 +	 * I think, in buf_prepare you only have to protect global data,
 +	 * the actual buffer is yours
 +	 */
 +	buf->inwork = 1;
 +
 +	if (buf->code	!= icd->current_fmt->code ||
 +	    vb->width	!= icd->user_width ||
 +	    vb->height	!= icd->user_height ||
 +	    vb->field	!= field) {
 +		buf->code	= icd->current_fmt->code;
 +		vb->width	= icd->user_width;
 +		vb->height	= icd->user_height;
 +		vb->field	= field;
 +		vb->state	= VIDEOBUF_NEEDS_INIT;
 +	}
 +
 +	vb->size = icd->sizeimage;
 +	if (0 != vb->baddr && vb->bsize < vb->size) {
 +		ret = -EINVAL;
 +		goto out;
 +	}
 +
 +	if (vb->state == VIDEOBUF_NEEDS_INIT) {
 +		ret = videobuf_iolock(vq, vb, NULL);
 +		if (ret)
 +			goto fail;
 +
 +		vb->state = VIDEOBUF_PREPARED;
 +	}
 +
 +	buf->inwork = 0;
 +
 +	return 0;
 +
 +fail:
 +	free_buffer(vq, buf);
 +out:
 +	buf->inwork = 0;
 +	return ret;
 +}
 +
 +static int mx1_camera_setup_dma(struct mx1_camera_dev *pcdev)
 +{
 +	struct videobuf_buffer *vbuf = &pcdev->active->vb;
 +	struct device *dev = pcdev->icd->parent;
 +	int ret;
 +
 +	if (unlikely(!pcdev->active)) {
 +		dev_err(dev, "DMA End IRQ with no active buffer\n");
 +		return -EFAULT;
 +	}
 +
 +	/* setup sg list for future DMA */
 +	ret = imx_dma_setup_single(pcdev->dma_chan,
 +		videobuf_to_dma_contig(vbuf),
 +		vbuf->size, pcdev->res->start +
 +		CSIRXR, DMA_MODE_READ);
 +	if (unlikely(ret))
 +		dev_err(dev, "Failed to setup DMA sg list\n");
 +
 +	return ret;
 +}
 +
 +/* Called under spinlock_irqsave(&pcdev->lock, ...) */
 +static void mx1_videobuf_queue(struct videobuf_queue *vq,
 +						struct videobuf_buffer *vb)
 +{
 +	struct soc_camera_device *icd = vq->priv_data;
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx1_camera_dev *pcdev = ici->priv;
 +	struct mx1_buffer *buf = container_of(vb, struct mx1_buffer, vb);
 +
 +	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 +		vb, vb->baddr, vb->bsize);
 +
 +	list_add_tail(&vb->queue, &pcdev->capture);
 +
 +	vb->state = VIDEOBUF_ACTIVE;
 +
 +	if (!pcdev->active) {
 +		pcdev->active = buf;
 +
 +		/* setup sg list for future DMA */
 +		if (!mx1_camera_setup_dma(pcdev)) {
 +			unsigned int temp;
 +			/* enable SOF irq */
 +			temp = __raw_readl(pcdev->base + CSICR1) |
 +							CSICR1_SOF_INTEN;
 +			__raw_writel(temp, pcdev->base + CSICR1);
 +		}
 +	}
 +}
 +
 +static void mx1_videobuf_release(struct videobuf_queue *vq,
 +				 struct videobuf_buffer *vb)
 +{
 +	struct mx1_buffer *buf = container_of(vb, struct mx1_buffer, vb);
 +#ifdef DEBUG
 +	struct soc_camera_device *icd = vq->priv_data;
 +	struct device *dev = icd->parent;
 +
 +	dev_dbg(dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 +		vb, vb->baddr, vb->bsize);
 +
 +	switch (vb->state) {
 +	case VIDEOBUF_ACTIVE:
 +		dev_dbg(dev, "%s (active)\n", __func__);
 +		break;
 +	case VIDEOBUF_QUEUED:
 +		dev_dbg(dev, "%s (queued)\n", __func__);
 +		break;
 +	case VIDEOBUF_PREPARED:
 +		dev_dbg(dev, "%s (prepared)\n", __func__);
 +		break;
 +	default:
 +		dev_dbg(dev, "%s (unknown)\n", __func__);
 +		break;
 +	}
 +#endif
 +
 +	free_buffer(vq, buf);
 +}
 +
 +static void mx1_camera_wakeup(struct mx1_camera_dev *pcdev,
 +			      struct videobuf_buffer *vb,
 +			      struct mx1_buffer *buf)
 +{
 +	/* _init is used to debug races, see comment in mx1_camera_reqbufs() */
 +	list_del_init(&vb->queue);
 +	vb->state = VIDEOBUF_DONE;
 +	do_gettimeofday(&vb->ts);
 +	vb->field_count++;
 +	wake_up(&vb->done);
 +
 +	if (list_empty(&pcdev->capture)) {
 +		pcdev->active = NULL;
 +		return;
 +	}
 +
 +	pcdev->active = list_entry(pcdev->capture.next,
 +				   struct mx1_buffer, vb.queue);
 +
 +	/* setup sg list for future DMA */
 +	if (likely(!mx1_camera_setup_dma(pcdev))) {
 +		unsigned int temp;
 +
 +		/* enable SOF irq */
 +		temp = __raw_readl(pcdev->base + CSICR1) | CSICR1_SOF_INTEN;
 +		__raw_writel(temp, pcdev->base + CSICR1);
 +	}
 +}
 +
 +static void mx1_camera_dma_irq(int channel, void *data)
 +{
 +	struct mx1_camera_dev *pcdev = data;
 +	struct device *dev = pcdev->icd->parent;
 +	struct mx1_buffer *buf;
 +	struct videobuf_buffer *vb;
 +	unsigned long flags;
 +
 +	spin_lock_irqsave(&pcdev->lock, flags);
 +
 +	imx_dma_disable(channel);
 +
 +	if (unlikely(!pcdev->active)) {
 +		dev_err(dev, "DMA End IRQ with no active buffer\n");
 +		goto out;
 +	}
 +
 +	vb = &pcdev->active->vb;
 +	buf = container_of(vb, struct mx1_buffer, vb);
 +	WARN_ON(buf->inwork || list_empty(&vb->queue));
 +	dev_dbg(dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 +		vb, vb->baddr, vb->bsize);
 +
 +	mx1_camera_wakeup(pcdev, vb, buf);
 +out:
 +	spin_unlock_irqrestore(&pcdev->lock, flags);
 +}
 +
 +static struct videobuf_queue_ops mx1_videobuf_ops = {
 +	.buf_setup	= mx1_videobuf_setup,
 +	.buf_prepare	= mx1_videobuf_prepare,
 +	.buf_queue	= mx1_videobuf_queue,
 +	.buf_release	= mx1_videobuf_release,
 +};
 +
 +static void mx1_camera_init_videobuf(struct videobuf_queue *q,
 +				     struct soc_camera_device *icd)
 +{
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx1_camera_dev *pcdev = ici->priv;
 +
 +	videobuf_queue_dma_contig_init(q, &mx1_videobuf_ops, icd->parent,
 +				&pcdev->lock, V4L2_BUF_TYPE_VIDEO_CAPTURE,
 +				V4L2_FIELD_NONE,
 +				sizeof(struct mx1_buffer), icd, &icd->video_lock);
 +}
 +
 +static int mclk_get_divisor(struct mx1_camera_dev *pcdev)
 +{
 +	unsigned int mclk = pcdev->mclk;
 +	unsigned long div;
 +	unsigned long lcdclk;
 +
 +	lcdclk = clk_get_rate(pcdev->clk);
 +
 +	/*
 +	 * We verify platform_mclk_10khz != 0, so if anyone breaks it, here
 +	 * they get a nice Oops
 +	 */
 +	div = (lcdclk + 2 * mclk - 1) / (2 * mclk) - 1;
 +
 +	dev_dbg(pcdev->icd->parent,
 +		"System clock %lukHz, target freq %dkHz, divisor %lu\n",
 +		lcdclk / 1000, mclk / 1000, div);
 +
 +	return div;
 +}
 +
 +static void mx1_camera_activate(struct mx1_camera_dev *pcdev)
 +{
 +	unsigned int csicr1 = CSICR1_EN;
 +
 +	dev_dbg(pcdev->icd->parent, "Activate device\n");
 +
 +	clk_prepare_enable(pcdev->clk);
 +
 +	/* enable CSI before doing anything else */
 +	__raw_writel(csicr1, pcdev->base + CSICR1);
 +
 +	csicr1 |= CSICR1_MCLKEN | CSICR1_FCC | CSICR1_GCLK_MODE;
 +	csicr1 |= CSICR1_MCLKDIV(mclk_get_divisor(pcdev));
 +	csicr1 |= CSICR1_RXFF_LEVEL(2); /* 16 words */
 +
 +	__raw_writel(csicr1, pcdev->base + CSICR1);
 +}
 +
 +static void mx1_camera_deactivate(struct mx1_camera_dev *pcdev)
 +{
 +	dev_dbg(pcdev->icd->parent, "Deactivate device\n");
 +
 +	/* Disable all CSI interface */
 +	__raw_writel(0x00, pcdev->base + CSICR1);
 +
 +	clk_disable_unprepare(pcdev->clk);
 +}
 +
 +/*
 + * The following two functions absolutely depend on the fact, that
 + * there can be only one camera on i.MX1/i.MXL camera sensor interface
 + */
 +static int mx1_camera_add_device(struct soc_camera_device *icd)
 +{
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx1_camera_dev *pcdev = ici->priv;
 +
 +	if (pcdev->icd)
 +		return -EBUSY;
 +
 +	dev_info(icd->parent, "MX1 Camera driver attached to camera %d\n",
 +		 icd->devnum);
 +
 +	mx1_camera_activate(pcdev);
 +
 +	pcdev->icd = icd;
 +
 +	return 0;
 +}
 +
 +static void mx1_camera_remove_device(struct soc_camera_device *icd)
 +{
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx1_camera_dev *pcdev = ici->priv;
 +	unsigned int csicr1;
 +
 +	BUG_ON(icd != pcdev->icd);
 +
 +	/* disable interrupts */
 +	csicr1 = __raw_readl(pcdev->base + CSICR1) & ~CSI_IRQ_MASK;
 +	__raw_writel(csicr1, pcdev->base + CSICR1);
 +
 +	/* Stop DMA engine */
 +	imx_dma_disable(pcdev->dma_chan);
 +
 +	dev_info(icd->parent, "MX1 Camera driver detached from camera %d\n",
 +		 icd->devnum);
 +
 +	mx1_camera_deactivate(pcdev);
 +
 +	pcdev->icd = NULL;
 +}
 +
 +static int mx1_camera_set_crop(struct soc_camera_device *icd,
 +			       struct v4l2_crop *a)
 +{
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +
 +	return v4l2_subdev_call(sd, video, s_crop, a);
 +}
 +
 +static int mx1_camera_set_bus_param(struct soc_camera_device *icd)
 +{
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx1_camera_dev *pcdev = ici->priv;
 +	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
 +	unsigned long common_flags;
 +	unsigned int csicr1;
 +	int ret;
 +
 +	/* MX1 supports only 8bit buswidth */
 +	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
 +	if (!ret) {
 +		common_flags = soc_mbus_config_compatible(&cfg, CSI_BUS_FLAGS);
 +		if (!common_flags) {
 +			dev_warn(icd->parent,
 +				 "Flags incompatible: camera 0x%x, host 0x%x\n",
 +				 cfg.flags, CSI_BUS_FLAGS);
 +			return -EINVAL;
 +		}
 +	} else if (ret != -ENOIOCTLCMD) {
 +		return ret;
 +	} else {
 +		common_flags = CSI_BUS_FLAGS;
 +	}
 +
 +	/* Make choises, based on platform choice */
 +	if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
 +		(common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
 +			if (!pcdev->pdata ||
 +			     pcdev->pdata->flags & MX1_CAMERA_VSYNC_HIGH)
 +				common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
 +			else
 +				common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
 +	}
 +
 +	if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
 +		(common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
 +			if (!pcdev->pdata ||
 +			     pcdev->pdata->flags & MX1_CAMERA_PCLK_RISING)
 +				common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
 +			else
 +				common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
 +	}
 +
 +	if ((common_flags & V4L2_MBUS_DATA_ACTIVE_HIGH) &&
 +		(common_flags & V4L2_MBUS_DATA_ACTIVE_LOW)) {
 +			if (!pcdev->pdata ||
 +			     pcdev->pdata->flags & MX1_CAMERA_DATA_HIGH)
 +				common_flags &= ~V4L2_MBUS_DATA_ACTIVE_LOW;
 +			else
 +				common_flags &= ~V4L2_MBUS_DATA_ACTIVE_HIGH;
 +	}
 +
 +	cfg.flags = common_flags;
 +	ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
 +	if (ret < 0 && ret != -ENOIOCTLCMD) {
 +		dev_dbg(icd->parent, "camera s_mbus_config(0x%lx) returned %d\n",
 +			common_flags, ret);
 +		return ret;
 +	}
 +
 +	csicr1 = __raw_readl(pcdev->base + CSICR1);
 +
 +	if (common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
 +		csicr1 |= CSICR1_REDGE;
 +	if (common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
 +		csicr1 |= CSICR1_SOF_POL;
 +	if (common_flags & V4L2_MBUS_DATA_ACTIVE_LOW)
 +		csicr1 |= CSICR1_DATA_POL;
 +
 +	__raw_writel(csicr1, pcdev->base + CSICR1);
 +
 +	return 0;
 +}
 +
 +static int mx1_camera_set_fmt(struct soc_camera_device *icd,
 +			      struct v4l2_format *f)
 +{
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	const struct soc_camera_format_xlate *xlate;
 +	struct v4l2_pix_format *pix = &f->fmt.pix;
 +	struct v4l2_mbus_framefmt mf;
 +	int ret, buswidth;
 +
 +	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
 +	if (!xlate) {
 +		dev_warn(icd->parent, "Format %x not found\n",
 +			 pix->pixelformat);
 +		return -EINVAL;
 +	}
 +
 +	buswidth = xlate->host_fmt->bits_per_sample;
 +	if (buswidth > 8) {
 +		dev_warn(icd->parent,
 +			 "bits-per-sample %d for format %x unsupported\n",
 +			 buswidth, pix->pixelformat);
 +		return -EINVAL;
 +	}
 +
 +	mf.width	= pix->width;
 +	mf.height	= pix->height;
 +	mf.field	= pix->field;
 +	mf.colorspace	= pix->colorspace;
 +	mf.code		= xlate->code;
 +
 +	ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
 +	if (ret < 0)
 +		return ret;
 +
 +	if (mf.code != xlate->code)
 +		return -EINVAL;
 +
 +	pix->width		= mf.width;
 +	pix->height		= mf.height;
 +	pix->field		= mf.field;
 +	pix->colorspace		= mf.colorspace;
 +	icd->current_fmt	= xlate;
 +
 +	return ret;
 +}
 +
 +static int mx1_camera_try_fmt(struct soc_camera_device *icd,
 +			      struct v4l2_format *f)
 +{
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	const struct soc_camera_format_xlate *xlate;
 +	struct v4l2_pix_format *pix = &f->fmt.pix;
 +	struct v4l2_mbus_framefmt mf;
 +	int ret;
 +	/* TODO: limit to mx1 hardware capabilities */
 +
 +	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
 +	if (!xlate) {
 +		dev_warn(icd->parent, "Format %x not found\n",
 +			 pix->pixelformat);
 +		return -EINVAL;
 +	}
 +
 +	mf.width	= pix->width;
 +	mf.height	= pix->height;
 +	mf.field	= pix->field;
 +	mf.colorspace	= pix->colorspace;
 +	mf.code		= xlate->code;
 +
 +	/* limit to sensor capabilities */
 +	ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
 +	if (ret < 0)
 +		return ret;
 +
 +	pix->width	= mf.width;
 +	pix->height	= mf.height;
 +	pix->field	= mf.field;
 +	pix->colorspace	= mf.colorspace;
 +
 +	return 0;
 +}
 +
 +static int mx1_camera_reqbufs(struct soc_camera_device *icd,
 +			      struct v4l2_requestbuffers *p)
 +{
 +	int i;
 +
 +	/*
 +	 * This is for locking debugging only. I removed spinlocks and now I
 +	 * check whether .prepare is ever called on a linked buffer, or whether
 +	 * a dma IRQ can occur for an in-work or unlinked buffer. Until now
 +	 * it hadn't triggered
 +	 */
 +	for (i = 0; i < p->count; i++) {
 +		struct mx1_buffer *buf = container_of(icd->vb_vidq.bufs[i],
 +						      struct mx1_buffer, vb);
 +		buf->inwork = 0;
 +		INIT_LIST_HEAD(&buf->vb.queue);
 +	}
 +
 +	return 0;
 +}
 +
 +static unsigned int mx1_camera_poll(struct file *file, poll_table *pt)
 +{
 +	struct soc_camera_device *icd = file->private_data;
 +	struct mx1_buffer *buf;
 +
 +	buf = list_entry(icd->vb_vidq.stream.next, struct mx1_buffer,
 +			 vb.stream);
 +
 +	poll_wait(file, &buf->vb.done, pt);
 +
 +	if (buf->vb.state == VIDEOBUF_DONE ||
 +	    buf->vb.state == VIDEOBUF_ERROR)
 +		return POLLIN | POLLRDNORM;
 +
 +	return 0;
 +}
 +
 +static int mx1_camera_querycap(struct soc_camera_host *ici,
 +			       struct v4l2_capability *cap)
 +{
 +	/* cap->name is set by the friendly caller:-> */
 +	strlcpy(cap->card, "i.MX1/i.MXL Camera", sizeof(cap->card));
 +	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 +
 +	return 0;
 +}
 +
 +static struct soc_camera_host_ops mx1_soc_camera_host_ops = {
 +	.owner		= THIS_MODULE,
 +	.add		= mx1_camera_add_device,
 +	.remove		= mx1_camera_remove_device,
 +	.set_bus_param	= mx1_camera_set_bus_param,
 +	.set_crop	= mx1_camera_set_crop,
 +	.set_fmt	= mx1_camera_set_fmt,
 +	.try_fmt	= mx1_camera_try_fmt,
 +	.init_videobuf	= mx1_camera_init_videobuf,
 +	.reqbufs	= mx1_camera_reqbufs,
 +	.poll		= mx1_camera_poll,
 +	.querycap	= mx1_camera_querycap,
 +};
 +
 +static struct fiq_handler fh = {
 +	.name		= "csi_sof"
 +};
 +
 +static int __init mx1_camera_probe(struct platform_device *pdev)
 +{
 +	struct mx1_camera_dev *pcdev;
 +	struct resource *res;
 +	struct pt_regs regs;
 +	struct clk *clk;
 +	void __iomem *base;
 +	unsigned int irq;
 +	int err = 0;
 +
 +	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 +	irq = platform_get_irq(pdev, 0);
 +	if (!res || (int)irq <= 0) {
 +		err = -ENODEV;
 +		goto exit;
 +	}
 +
 +	clk = clk_get(&pdev->dev, "csi_clk");
 +	if (IS_ERR(clk)) {
 +		err = PTR_ERR(clk);
 +		goto exit;
 +	}
 +
 +	pcdev = kzalloc(sizeof(*pcdev), GFP_KERNEL);
 +	if (!pcdev) {
 +		dev_err(&pdev->dev, "Could not allocate pcdev\n");
 +		err = -ENOMEM;
 +		goto exit_put_clk;
 +	}
 +
 +	pcdev->res = res;
 +	pcdev->clk = clk;
 +
 +	pcdev->pdata = pdev->dev.platform_data;
 +
 +	if (pcdev->pdata)
 +		pcdev->mclk = pcdev->pdata->mclk_10khz * 10000;
 +
 +	if (!pcdev->mclk) {
 +		dev_warn(&pdev->dev,
 +			 "mclk_10khz == 0! Please, fix your platform data. "
 +			 "Using default 20MHz\n");
 +		pcdev->mclk = 20000000;
 +	}
 +
 +	INIT_LIST_HEAD(&pcdev->capture);
 +	spin_lock_init(&pcdev->lock);
 +
 +	/*
 +	 * Request the regions.
 +	 */
 +	if (!request_mem_region(res->start, resource_size(res), DRIVER_NAME)) {
 +		err = -EBUSY;
 +		goto exit_kfree;
 +	}
 +
 +	base = ioremap(res->start, resource_size(res));
 +	if (!base) {
 +		err = -ENOMEM;
 +		goto exit_release;
 +	}
 +	pcdev->irq = irq;
 +	pcdev->base = base;
 +
 +	/* request dma */
 +	pcdev->dma_chan = imx_dma_request_by_prio(DRIVER_NAME, DMA_PRIO_HIGH);
 +	if (pcdev->dma_chan < 0) {
 +		dev_err(&pdev->dev, "Can't request DMA for MX1 CSI\n");
 +		err = -EBUSY;
 +		goto exit_iounmap;
 +	}
 +	dev_dbg(&pdev->dev, "got DMA channel %d\n", pcdev->dma_chan);
 +
 +	imx_dma_setup_handlers(pcdev->dma_chan, mx1_camera_dma_irq, NULL,
 +			       pcdev);
 +
 +	imx_dma_config_channel(pcdev->dma_chan, IMX_DMA_TYPE_FIFO,
 +			       IMX_DMA_MEMSIZE_32, MX1_DMA_REQ_CSI_R, 0);
 +	/* burst length : 16 words = 64 bytes */
 +	imx_dma_config_burstlen(pcdev->dma_chan, 0);
 +
 +	/* request irq */
 +	err = claim_fiq(&fh);
 +	if (err) {
 +		dev_err(&pdev->dev, "Camera interrupt register failed \n");
 +		goto exit_free_dma;
 +	}
 +
 +	set_fiq_handler(&mx1_camera_sof_fiq_start, &mx1_camera_sof_fiq_end -
 +						   &mx1_camera_sof_fiq_start);
 +
 +	regs.ARM_r8 = (long)MX1_DMA_DIMR;
 +	regs.ARM_r9 = (long)MX1_DMA_CCR(pcdev->dma_chan);
 +	regs.ARM_r10 = (long)pcdev->base + CSICR1;
 +	regs.ARM_fp = (long)pcdev->base + CSISR;
 +	regs.ARM_sp = 1 << pcdev->dma_chan;
 +	set_fiq_regs(&regs);
 +
 +	mxc_set_irq_fiq(irq, 1);
 +	enable_fiq(irq);
 +
 +	pcdev->soc_host.drv_name	= DRIVER_NAME;
 +	pcdev->soc_host.ops		= &mx1_soc_camera_host_ops;
 +	pcdev->soc_host.priv		= pcdev;
 +	pcdev->soc_host.v4l2_dev.dev	= &pdev->dev;
 +	pcdev->soc_host.nr		= pdev->id;
 +	err = soc_camera_host_register(&pcdev->soc_host);
 +	if (err)
 +		goto exit_free_irq;
 +
 +	dev_info(&pdev->dev, "MX1 Camera driver loaded\n");
 +
 +	return 0;
 +
 +exit_free_irq:
 +	disable_fiq(irq);
 +	mxc_set_irq_fiq(irq, 0);
 +	release_fiq(&fh);
 +exit_free_dma:
 +	imx_dma_free(pcdev->dma_chan);
 +exit_iounmap:
 +	iounmap(base);
 +exit_release:
 +	release_mem_region(res->start, resource_size(res));
 +exit_kfree:
 +	kfree(pcdev);
 +exit_put_clk:
 +	clk_put(clk);
 +exit:
 +	return err;
 +}
 +
 +static int __exit mx1_camera_remove(struct platform_device *pdev)
 +{
 +	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
 +	struct mx1_camera_dev *pcdev = container_of(soc_host,
 +					struct mx1_camera_dev, soc_host);
 +	struct resource *res;
 +
 +	imx_dma_free(pcdev->dma_chan);
 +	disable_fiq(pcdev->irq);
 +	mxc_set_irq_fiq(pcdev->irq, 0);
 +	release_fiq(&fh);
 +
 +	clk_put(pcdev->clk);
 +
 +	soc_camera_host_unregister(soc_host);
 +
 +	iounmap(pcdev->base);
 +
 +	res = pcdev->res;
 +	release_mem_region(res->start, resource_size(res));
 +
 +	kfree(pcdev);
 +
 +	dev_info(&pdev->dev, "MX1 Camera driver unloaded\n");
 +
 +	return 0;
 +}
 +
 +static struct platform_driver mx1_camera_driver = {
 +	.driver 	= {
 +		.name	= DRIVER_NAME,
 +	},
 +	.remove		= __exit_p(mx1_camera_remove),
 +};
 +
 +static int __init mx1_camera_init(void)
 +{
 +	return platform_driver_probe(&mx1_camera_driver, mx1_camera_probe);
 +}
 +
 +static void __exit mx1_camera_exit(void)
 +{
 +	return platform_driver_unregister(&mx1_camera_driver);
 +}
 +
 +module_init(mx1_camera_init);
 +module_exit(mx1_camera_exit);
 +
 +MODULE_DESCRIPTION("i.MX1/i.MXL SoC Camera Host driver");
 +MODULE_AUTHOR("Paulius Zaleckas <paulius.zaleckas@teltonika.lt>");
 +MODULE_LICENSE("GPL v2");
 +MODULE_VERSION(DRIVER_VERSION);
 +MODULE_ALIAS("platform:" DRIVER_NAME);
diff --cc drivers/media/platform/soc_camera/mx2_camera.c
index 619df35de3da,000000000000..403d7f17bfab
mode 100644,000000..100644
--- a/drivers/media/platform/soc_camera/mx2_camera.c
+++ b/drivers/media/platform/soc_camera/mx2_camera.c
@@@ -1,1863 -1,0 +1,1863 @@@
 +/*
 + * V4L2 Driver for i.MX27/i.MX25 camera host
 + *
 + * Copyright (C) 2008, Sascha Hauer, Pengutronix
 + * Copyright (C) 2010, Baruch Siach, Orex Computed Radiography
 + * Copyright (C) 2012, Javier Martin, Vista Silicon S.L.
 + *
 + * This program is free software; you can redistribute it and/or modify
 + * it under the terms of the GNU General Public License as published by
 + * the Free Software Foundation; either version 2 of the License, or
 + * (at your option) any later version.
 + */
 +
 +#include <linux/init.h>
 +#include <linux/module.h>
 +#include <linux/io.h>
 +#include <linux/delay.h>
 +#include <linux/slab.h>
 +#include <linux/dma-mapping.h>
 +#include <linux/errno.h>
 +#include <linux/fs.h>
 +#include <linux/gcd.h>
 +#include <linux/interrupt.h>
 +#include <linux/kernel.h>
 +#include <linux/math64.h>
 +#include <linux/mm.h>
 +#include <linux/moduleparam.h>
 +#include <linux/time.h>
 +#include <linux/device.h>
 +#include <linux/platform_device.h>
 +#include <linux/mutex.h>
 +#include <linux/clk.h>
 +
 +#include <media/v4l2-common.h>
 +#include <media/v4l2-dev.h>
 +#include <media/videobuf2-core.h>
 +#include <media/videobuf2-dma-contig.h>
 +#include <media/soc_camera.h>
 +#include <media/soc_mediabus.h>
 +
 +#include <linux/videodev2.h>
 +
- #include <mach/mx2_cam.h>
++#include <linux/platform_data/camera-mx2.h>
 +#include <mach/hardware.h>
 +
 +#include <asm/dma.h>
 +
 +#define MX2_CAM_DRV_NAME "mx2-camera"
 +#define MX2_CAM_VERSION "0.0.6"
 +#define MX2_CAM_DRIVER_DESCRIPTION "i.MX2x_Camera"
 +
 +/* reset values */
 +#define CSICR1_RESET_VAL	0x40000800
 +#define CSICR2_RESET_VAL	0x0
 +#define CSICR3_RESET_VAL	0x0
 +
 +/* csi control reg 1 */
 +#define CSICR1_SWAP16_EN	(1 << 31)
 +#define CSICR1_EXT_VSYNC	(1 << 30)
 +#define CSICR1_EOF_INTEN	(1 << 29)
 +#define CSICR1_PRP_IF_EN	(1 << 28)
 +#define CSICR1_CCIR_MODE	(1 << 27)
 +#define CSICR1_COF_INTEN	(1 << 26)
 +#define CSICR1_SF_OR_INTEN	(1 << 25)
 +#define CSICR1_RF_OR_INTEN	(1 << 24)
 +#define CSICR1_STATFF_LEVEL	(3 << 22)
 +#define CSICR1_STATFF_INTEN	(1 << 21)
 +#define CSICR1_RXFF_LEVEL(l)	(((l) & 3) << 19)	/* MX27 */
 +#define CSICR1_FB2_DMA_INTEN	(1 << 20)		/* MX25 */
 +#define CSICR1_FB1_DMA_INTEN	(1 << 19)		/* MX25 */
 +#define CSICR1_RXFF_INTEN	(1 << 18)
 +#define CSICR1_SOF_POL		(1 << 17)
 +#define CSICR1_SOF_INTEN	(1 << 16)
 +#define CSICR1_MCLKDIV(d)	(((d) & 0xF) << 12)
 +#define CSICR1_HSYNC_POL	(1 << 11)
 +#define CSICR1_CCIR_EN		(1 << 10)
 +#define CSICR1_MCLKEN		(1 << 9)
 +#define CSICR1_FCC		(1 << 8)
 +#define CSICR1_PACK_DIR		(1 << 7)
 +#define CSICR1_CLR_STATFIFO	(1 << 6)
 +#define CSICR1_CLR_RXFIFO	(1 << 5)
 +#define CSICR1_GCLK_MODE	(1 << 4)
 +#define CSICR1_INV_DATA		(1 << 3)
 +#define CSICR1_INV_PCLK		(1 << 2)
 +#define CSICR1_REDGE		(1 << 1)
 +#define CSICR1_FMT_MASK		(CSICR1_PACK_DIR | CSICR1_SWAP16_EN)
 +
 +#define SHIFT_STATFF_LEVEL	22
 +#define SHIFT_RXFF_LEVEL	19
 +#define SHIFT_MCLKDIV		12
 +
 +/* control reg 3 */
 +#define CSICR3_FRMCNT		(0xFFFF << 16)
 +#define CSICR3_FRMCNT_RST	(1 << 15)
 +#define CSICR3_DMA_REFLASH_RFF	(1 << 14)
 +#define CSICR3_DMA_REFLASH_SFF	(1 << 13)
 +#define CSICR3_DMA_REQ_EN_RFF	(1 << 12)
 +#define CSICR3_DMA_REQ_EN_SFF	(1 << 11)
 +#define CSICR3_RXFF_LEVEL(l)	(((l) & 7) << 4)	/* MX25 */
 +#define CSICR3_CSI_SUP		(1 << 3)
 +#define CSICR3_ZERO_PACK_EN	(1 << 2)
 +#define CSICR3_ECC_INT_EN	(1 << 1)
 +#define CSICR3_ECC_AUTO_EN	(1 << 0)
 +
 +#define SHIFT_FRMCNT		16
 +
 +/* csi status reg */
 +#define CSISR_SFF_OR_INT	(1 << 25)
 +#define CSISR_RFF_OR_INT	(1 << 24)
 +#define CSISR_STATFF_INT	(1 << 21)
 +#define CSISR_DMA_TSF_FB2_INT	(1 << 20)	/* MX25 */
 +#define CSISR_DMA_TSF_FB1_INT	(1 << 19)	/* MX25 */
 +#define CSISR_RXFF_INT		(1 << 18)
 +#define CSISR_EOF_INT		(1 << 17)
 +#define CSISR_SOF_INT		(1 << 16)
 +#define CSISR_F2_INT		(1 << 15)
 +#define CSISR_F1_INT		(1 << 14)
 +#define CSISR_COF_INT		(1 << 13)
 +#define CSISR_ECC_INT		(1 << 1)
 +#define CSISR_DRDY		(1 << 0)
 +
 +#define CSICR1			0x00
 +#define CSICR2			0x04
 +#define CSISR			(cpu_is_mx27() ? 0x08 : 0x18)
 +#define CSISTATFIFO		0x0c
 +#define CSIRFIFO		0x10
 +#define CSIRXCNT		0x14
 +#define CSICR3			(cpu_is_mx27() ? 0x1C : 0x08)
 +#define CSIDMASA_STATFIFO	0x20
 +#define CSIDMATA_STATFIFO	0x24
 +#define CSIDMASA_FB1		0x28
 +#define CSIDMASA_FB2		0x2c
 +#define CSIFBUF_PARA		0x30
 +#define CSIIMAG_PARA		0x34
 +
 +/* EMMA PrP */
 +#define PRP_CNTL			0x00
 +#define PRP_INTR_CNTL			0x04
 +#define PRP_INTRSTATUS			0x08
 +#define PRP_SOURCE_Y_PTR		0x0c
 +#define PRP_SOURCE_CB_PTR		0x10
 +#define PRP_SOURCE_CR_PTR		0x14
 +#define PRP_DEST_RGB1_PTR		0x18
 +#define PRP_DEST_RGB2_PTR		0x1c
 +#define PRP_DEST_Y_PTR			0x20
 +#define PRP_DEST_CB_PTR			0x24
 +#define PRP_DEST_CR_PTR			0x28
 +#define PRP_SRC_FRAME_SIZE		0x2c
 +#define PRP_DEST_CH1_LINE_STRIDE	0x30
 +#define PRP_SRC_PIXEL_FORMAT_CNTL	0x34
 +#define PRP_CH1_PIXEL_FORMAT_CNTL	0x38
 +#define PRP_CH1_OUT_IMAGE_SIZE		0x3c
 +#define PRP_CH2_OUT_IMAGE_SIZE		0x40
 +#define PRP_SRC_LINE_STRIDE		0x44
 +#define PRP_CSC_COEF_012		0x48
 +#define PRP_CSC_COEF_345		0x4c
 +#define PRP_CSC_COEF_678		0x50
 +#define PRP_CH1_RZ_HORI_COEF1		0x54
 +#define PRP_CH1_RZ_HORI_COEF2		0x58
 +#define PRP_CH1_RZ_HORI_VALID		0x5c
 +#define PRP_CH1_RZ_VERT_COEF1		0x60
 +#define PRP_CH1_RZ_VERT_COEF2		0x64
 +#define PRP_CH1_RZ_VERT_VALID		0x68
 +#define PRP_CH2_RZ_HORI_COEF1		0x6c
 +#define PRP_CH2_RZ_HORI_COEF2		0x70
 +#define PRP_CH2_RZ_HORI_VALID		0x74
 +#define PRP_CH2_RZ_VERT_COEF1		0x78
 +#define PRP_CH2_RZ_VERT_COEF2		0x7c
 +#define PRP_CH2_RZ_VERT_VALID		0x80
 +
 +#define PRP_CNTL_CH1EN		(1 << 0)
 +#define PRP_CNTL_CH2EN		(1 << 1)
 +#define PRP_CNTL_CSIEN		(1 << 2)
 +#define PRP_CNTL_DATA_IN_YUV420	(0 << 3)
 +#define PRP_CNTL_DATA_IN_YUV422	(1 << 3)
 +#define PRP_CNTL_DATA_IN_RGB16	(2 << 3)
 +#define PRP_CNTL_DATA_IN_RGB32	(3 << 3)
 +#define PRP_CNTL_CH1_OUT_RGB8	(0 << 5)
 +#define PRP_CNTL_CH1_OUT_RGB16	(1 << 5)
 +#define PRP_CNTL_CH1_OUT_RGB32	(2 << 5)
 +#define PRP_CNTL_CH1_OUT_YUV422	(3 << 5)
 +#define PRP_CNTL_CH2_OUT_YUV420	(0 << 7)
 +#define PRP_CNTL_CH2_OUT_YUV422 (1 << 7)
 +#define PRP_CNTL_CH2_OUT_YUV444	(2 << 7)
 +#define PRP_CNTL_CH1_LEN	(1 << 9)
 +#define PRP_CNTL_CH2_LEN	(1 << 10)
 +#define PRP_CNTL_SKIP_FRAME	(1 << 11)
 +#define PRP_CNTL_SWRST		(1 << 12)
 +#define PRP_CNTL_CLKEN		(1 << 13)
 +#define PRP_CNTL_WEN		(1 << 14)
 +#define PRP_CNTL_CH1BYP		(1 << 15)
 +#define PRP_CNTL_IN_TSKIP(x)	((x) << 16)
 +#define PRP_CNTL_CH1_TSKIP(x)	((x) << 19)
 +#define PRP_CNTL_CH2_TSKIP(x)	((x) << 22)
 +#define PRP_CNTL_INPUT_FIFO_LEVEL(x)	((x) << 25)
 +#define PRP_CNTL_RZ_FIFO_LEVEL(x)	((x) << 27)
 +#define PRP_CNTL_CH2B1EN	(1 << 29)
 +#define PRP_CNTL_CH2B2EN	(1 << 30)
 +#define PRP_CNTL_CH2FEN		(1 << 31)
 +
 +/* IRQ Enable and status register */
 +#define PRP_INTR_RDERR		(1 << 0)
 +#define PRP_INTR_CH1WERR	(1 << 1)
 +#define PRP_INTR_CH2WERR	(1 << 2)
 +#define PRP_INTR_CH1FC		(1 << 3)
 +#define PRP_INTR_CH2FC		(1 << 5)
 +#define PRP_INTR_LBOVF		(1 << 7)
 +#define PRP_INTR_CH2OVF		(1 << 8)
 +
 +/* Resizing registers */
 +#define PRP_RZ_VALID_TBL_LEN(x)	((x) << 24)
 +#define PRP_RZ_VALID_BILINEAR	(1 << 31)
 +
 +#define MAX_VIDEO_MEM	16
 +
 +#define RESIZE_NUM_MIN	1
 +#define RESIZE_NUM_MAX	20
 +#define BC_COEF		3
 +#define SZ_COEF		(1 << BC_COEF)
 +
 +#define RESIZE_DIR_H	0
 +#define RESIZE_DIR_V	1
 +
 +#define RESIZE_ALGO_BILINEAR 0
 +#define RESIZE_ALGO_AVERAGING 1
 +
 +struct mx2_prp_cfg {
 +	int channel;
 +	u32 in_fmt;
 +	u32 out_fmt;
 +	u32 src_pixel;
 +	u32 ch1_pixel;
 +	u32 irq_flags;
 +	u32 csicr1;
 +};
 +
 +/* prp resizing parameters */
 +struct emma_prp_resize {
 +	int		algo; /* type of algorithm used */
 +	int		len; /* number of coefficients */
 +	unsigned char	s[RESIZE_NUM_MAX]; /* table of coefficients */
 +};
 +
 +/* prp configuration for a client-host fmt pair */
 +struct mx2_fmt_cfg {
 +	enum v4l2_mbus_pixelcode	in_fmt;
 +	u32				out_fmt;
 +	struct mx2_prp_cfg		cfg;
 +};
 +
 +enum mx2_buffer_state {
 +	MX2_STATE_QUEUED,
 +	MX2_STATE_ACTIVE,
 +	MX2_STATE_DONE,
 +};
 +
 +struct mx2_buf_internal {
 +	struct list_head	queue;
 +	int			bufnum;
 +	bool			discard;
 +};
 +
 +/* buffer for one video frame */
 +struct mx2_buffer {
 +	/* common v4l buffer stuff -- must be first */
 +	struct vb2_buffer		vb;
 +	enum mx2_buffer_state		state;
 +	struct mx2_buf_internal		internal;
 +};
 +
 +struct mx2_camera_dev {
 +	struct device		*dev;
 +	struct soc_camera_host	soc_host;
 +	struct soc_camera_device *icd;
 +	struct clk		*clk_csi, *clk_emma_ahb, *clk_emma_ipg;
 +
 +	void __iomem		*base_csi, *base_emma;
 +
 +	struct mx2_camera_platform_data *pdata;
 +	unsigned long		platform_flags;
 +
 +	struct list_head	capture;
 +	struct list_head	active_bufs;
 +	struct list_head	discard;
 +
 +	spinlock_t		lock;
 +
 +	int			dma;
 +	struct mx2_buffer	*active;
 +	struct mx2_buffer	*fb1_active;
 +	struct mx2_buffer	*fb2_active;
 +
 +	u32			csicr1;
 +
 +	struct mx2_buf_internal buf_discard[2];
 +	void			*discard_buffer;
 +	dma_addr_t		discard_buffer_dma;
 +	size_t			discard_size;
 +	struct mx2_fmt_cfg	*emma_prp;
 +	struct emma_prp_resize	resizing[2];
 +	unsigned int		s_width, s_height;
 +	u32			frame_count;
 +	struct vb2_alloc_ctx	*alloc_ctx;
 +};
 +
 +static struct mx2_buffer *mx2_ibuf_to_buf(struct mx2_buf_internal *int_buf)
 +{
 +	return container_of(int_buf, struct mx2_buffer, internal);
 +}
 +
 +static struct mx2_fmt_cfg mx27_emma_prp_table[] = {
 +	/*
 +	 * This is a generic configuration which is valid for most
 +	 * prp input-output format combinations.
 +	 * We set the incomming and outgoing pixelformat to a
 +	 * 16 Bit wide format and adjust the bytesperline
 +	 * accordingly. With this configuration the inputdata
 +	 * will not be changed by the emma and could be any type
 +	 * of 16 Bit Pixelformat.
 +	 */
 +	{
 +		.in_fmt		= 0,
 +		.out_fmt	= 0,
 +		.cfg		= {
 +			.channel	= 1,
 +			.in_fmt		= PRP_CNTL_DATA_IN_RGB16,
 +			.out_fmt	= PRP_CNTL_CH1_OUT_RGB16,
 +			.src_pixel	= 0x2ca00565, /* RGB565 */
 +			.ch1_pixel	= 0x2ca00565, /* RGB565 */
 +			.irq_flags	= PRP_INTR_RDERR | PRP_INTR_CH1WERR |
 +						PRP_INTR_CH1FC | PRP_INTR_LBOVF,
 +			.csicr1		= 0,
 +		}
 +	},
 +	{
 +		.in_fmt		= V4L2_MBUS_FMT_UYVY8_2X8,
 +		.out_fmt	= V4L2_PIX_FMT_YUYV,
 +		.cfg		= {
 +			.channel	= 1,
 +			.in_fmt		= PRP_CNTL_DATA_IN_YUV422,
 +			.out_fmt	= PRP_CNTL_CH1_OUT_YUV422,
 +			.src_pixel	= 0x22000888, /* YUV422 (YUYV) */
 +			.ch1_pixel	= 0x62000888, /* YUV422 (YUYV) */
 +			.irq_flags	= PRP_INTR_RDERR | PRP_INTR_CH1WERR |
 +						PRP_INTR_CH1FC | PRP_INTR_LBOVF,
 +			.csicr1		= CSICR1_SWAP16_EN,
 +		}
 +	},
 +	{
 +		.in_fmt		= V4L2_MBUS_FMT_YUYV8_2X8,
 +		.out_fmt	= V4L2_PIX_FMT_YUYV,
 +		.cfg		= {
 +			.channel	= 1,
 +			.in_fmt		= PRP_CNTL_DATA_IN_YUV422,
 +			.out_fmt	= PRP_CNTL_CH1_OUT_YUV422,
 +			.src_pixel	= 0x22000888, /* YUV422 (YUYV) */
 +			.ch1_pixel	= 0x62000888, /* YUV422 (YUYV) */
 +			.irq_flags	= PRP_INTR_RDERR | PRP_INTR_CH1WERR |
 +						PRP_INTR_CH1FC | PRP_INTR_LBOVF,
 +			.csicr1		= CSICR1_PACK_DIR,
 +		}
 +	},
 +	{
 +		.in_fmt		= V4L2_MBUS_FMT_YUYV8_2X8,
 +		.out_fmt	= V4L2_PIX_FMT_YUV420,
 +		.cfg		= {
 +			.channel	= 2,
 +			.in_fmt		= PRP_CNTL_DATA_IN_YUV422,
 +			.out_fmt	= PRP_CNTL_CH2_OUT_YUV420,
 +			.src_pixel	= 0x22000888, /* YUV422 (YUYV) */
 +			.irq_flags	= PRP_INTR_RDERR | PRP_INTR_CH2WERR |
 +					PRP_INTR_CH2FC | PRP_INTR_LBOVF |
 +					PRP_INTR_CH2OVF,
 +			.csicr1		= CSICR1_PACK_DIR,
 +		}
 +	},
 +	{
 +		.in_fmt		= V4L2_MBUS_FMT_UYVY8_2X8,
 +		.out_fmt	= V4L2_PIX_FMT_YUV420,
 +		.cfg		= {
 +			.channel	= 2,
 +			.in_fmt		= PRP_CNTL_DATA_IN_YUV422,
 +			.out_fmt	= PRP_CNTL_CH2_OUT_YUV420,
 +			.src_pixel	= 0x22000888, /* YUV422 (YUYV) */
 +			.irq_flags	= PRP_INTR_RDERR | PRP_INTR_CH2WERR |
 +					PRP_INTR_CH2FC | PRP_INTR_LBOVF |
 +					PRP_INTR_CH2OVF,
 +			.csicr1		= CSICR1_SWAP16_EN,
 +		}
 +	},
 +};
 +
 +static struct mx2_fmt_cfg *mx27_emma_prp_get_format(
 +					enum v4l2_mbus_pixelcode in_fmt,
 +					u32 out_fmt)
 +{
 +	int i;
 +
 +	for (i = 1; i < ARRAY_SIZE(mx27_emma_prp_table); i++)
 +		if ((mx27_emma_prp_table[i].in_fmt == in_fmt) &&
 +				(mx27_emma_prp_table[i].out_fmt == out_fmt)) {
 +			return &mx27_emma_prp_table[i];
 +		}
 +	/* If no match return the most generic configuration */
 +	return &mx27_emma_prp_table[0];
 +};
 +
 +static void mx27_update_emma_buf(struct mx2_camera_dev *pcdev,
 +				 unsigned long phys, int bufnum)
 +{
 +	struct mx2_fmt_cfg *prp = pcdev->emma_prp;
 +
 +	if (prp->cfg.channel == 1) {
 +		writel(phys, pcdev->base_emma +
 +				PRP_DEST_RGB1_PTR + 4 * bufnum);
 +	} else {
 +		writel(phys, pcdev->base_emma +
 +			PRP_DEST_Y_PTR - 0x14 * bufnum);
 +		if (prp->out_fmt == V4L2_PIX_FMT_YUV420) {
 +			u32 imgsize = pcdev->icd->user_height *
 +					pcdev->icd->user_width;
 +
 +			writel(phys + imgsize, pcdev->base_emma +
 +				PRP_DEST_CB_PTR - 0x14 * bufnum);
 +			writel(phys + ((5 * imgsize) / 4), pcdev->base_emma +
 +				PRP_DEST_CR_PTR - 0x14 * bufnum);
 +		}
 +	}
 +}
 +
 +static void mx2_camera_deactivate(struct mx2_camera_dev *pcdev)
 +{
 +	unsigned long flags;
 +
 +	clk_disable_unprepare(pcdev->clk_csi);
 +	writel(0, pcdev->base_csi + CSICR1);
 +	if (cpu_is_mx27()) {
 +		writel(0, pcdev->base_emma + PRP_CNTL);
 +	} else if (cpu_is_mx25()) {
 +		spin_lock_irqsave(&pcdev->lock, flags);
 +		pcdev->fb1_active = NULL;
 +		pcdev->fb2_active = NULL;
 +		writel(0, pcdev->base_csi + CSIDMASA_FB1);
 +		writel(0, pcdev->base_csi + CSIDMASA_FB2);
 +		spin_unlock_irqrestore(&pcdev->lock, flags);
 +	}
 +}
 +
 +/*
 + * The following two functions absolutely depend on the fact, that
 + * there can be only one camera on mx2 camera sensor interface
 + */
 +static int mx2_camera_add_device(struct soc_camera_device *icd)
 +{
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx2_camera_dev *pcdev = ici->priv;
 +	int ret;
 +	u32 csicr1;
 +
 +	if (pcdev->icd)
 +		return -EBUSY;
 +
 +	ret = clk_prepare_enable(pcdev->clk_csi);
 +	if (ret < 0)
 +		return ret;
 +
 +	csicr1 = CSICR1_MCLKEN;
 +
 +	if (cpu_is_mx27())
 +		csicr1 |= CSICR1_PRP_IF_EN | CSICR1_FCC |
 +			CSICR1_RXFF_LEVEL(0);
 +
 +	pcdev->csicr1 = csicr1;
 +	writel(pcdev->csicr1, pcdev->base_csi + CSICR1);
 +
 +	pcdev->icd = icd;
 +	pcdev->frame_count = 0;
 +
 +	dev_info(icd->parent, "Camera driver attached to camera %d\n",
 +		 icd->devnum);
 +
 +	return 0;
 +}
 +
 +static void mx2_camera_remove_device(struct soc_camera_device *icd)
 +{
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx2_camera_dev *pcdev = ici->priv;
 +
 +	BUG_ON(icd != pcdev->icd);
 +
 +	dev_info(icd->parent, "Camera driver detached from camera %d\n",
 +		 icd->devnum);
 +
 +	mx2_camera_deactivate(pcdev);
 +
 +	pcdev->icd = NULL;
 +}
 +
 +static void mx25_camera_frame_done(struct mx2_camera_dev *pcdev, int fb,
 +		int state)
 +{
 +	struct vb2_buffer *vb;
 +	struct mx2_buffer *buf;
 +	struct mx2_buffer **fb_active = fb == 1 ? &pcdev->fb1_active :
 +		&pcdev->fb2_active;
 +	u32 fb_reg = fb == 1 ? CSIDMASA_FB1 : CSIDMASA_FB2;
 +	unsigned long flags;
 +
 +	spin_lock_irqsave(&pcdev->lock, flags);
 +
 +	if (*fb_active == NULL)
 +		goto out;
 +
 +	vb = &(*fb_active)->vb;
 +	dev_dbg(pcdev->dev, "%s (vb=0x%p) 0x%p %lu\n", __func__,
 +		vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
 +
 +	do_gettimeofday(&vb->v4l2_buf.timestamp);
 +	vb->v4l2_buf.sequence++;
 +	vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
 +
 +	if (list_empty(&pcdev->capture)) {
 +		buf = NULL;
 +		writel(0, pcdev->base_csi + fb_reg);
 +	} else {
 +		buf = list_first_entry(&pcdev->capture, struct mx2_buffer,
 +				internal.queue);
 +		vb = &buf->vb;
 +		list_del(&buf->internal.queue);
 +		buf->state = MX2_STATE_ACTIVE;
 +		writel(vb2_dma_contig_plane_dma_addr(vb, 0),
 +		       pcdev->base_csi + fb_reg);
 +	}
 +
 +	*fb_active = buf;
 +
 +out:
 +	spin_unlock_irqrestore(&pcdev->lock, flags);
 +}
 +
 +static irqreturn_t mx25_camera_irq(int irq_csi, void *data)
 +{
 +	struct mx2_camera_dev *pcdev = data;
 +	u32 status = readl(pcdev->base_csi + CSISR);
 +
 +	if (status & CSISR_DMA_TSF_FB1_INT)
 +		mx25_camera_frame_done(pcdev, 1, MX2_STATE_DONE);
 +	else if (status & CSISR_DMA_TSF_FB2_INT)
 +		mx25_camera_frame_done(pcdev, 2, MX2_STATE_DONE);
 +
 +	/* FIXME: handle CSISR_RFF_OR_INT */
 +
 +	writel(status, pcdev->base_csi + CSISR);
 +
 +	return IRQ_HANDLED;
 +}
 +
 +/*
 + *  Videobuf operations
 + */
 +static int mx2_videobuf_setup(struct vb2_queue *vq,
 +			const struct v4l2_format *fmt,
 +			unsigned int *count, unsigned int *num_planes,
 +			unsigned int sizes[], void *alloc_ctxs[])
 +{
 +	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx2_camera_dev *pcdev = ici->priv;
 +
 +	dev_dbg(icd->parent, "count=%d, size=%d\n", *count, sizes[0]);
 +
 +	/* TODO: support for VIDIOC_CREATE_BUFS not ready */
 +	if (fmt != NULL)
 +		return -ENOTTY;
 +
 +	alloc_ctxs[0] = pcdev->alloc_ctx;
 +
 +	sizes[0] = icd->sizeimage;
 +
 +	if (0 == *count)
 +		*count = 32;
 +	if (!*num_planes &&
 +	    sizes[0] * *count > MAX_VIDEO_MEM * 1024 * 1024)
 +		*count = (MAX_VIDEO_MEM * 1024 * 1024) / sizes[0];
 +
 +	*num_planes = 1;
 +
 +	return 0;
 +}
 +
 +static int mx2_videobuf_prepare(struct vb2_buffer *vb)
 +{
 +	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
 +	int ret = 0;
 +
 +	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
 +		vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
 +
 +#ifdef DEBUG
 +	/*
 +	 * This can be useful if you want to see if we actually fill
 +	 * the buffer with something
 +	 */
 +	memset((void *)vb2_plane_vaddr(vb, 0),
 +	       0xaa, vb2_get_plane_payload(vb, 0));
 +#endif
 +
 +	vb2_set_plane_payload(vb, 0, icd->sizeimage);
 +	if (vb2_plane_vaddr(vb, 0) &&
 +	    vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0)) {
 +		ret = -EINVAL;
 +		goto out;
 +	}
 +
 +	return 0;
 +
 +out:
 +	return ret;
 +}
 +
 +static void mx2_videobuf_queue(struct vb2_buffer *vb)
 +{
 +	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
 +	struct soc_camera_host *ici =
 +		to_soc_camera_host(icd->parent);
 +	struct mx2_camera_dev *pcdev = ici->priv;
 +	struct mx2_buffer *buf = container_of(vb, struct mx2_buffer, vb);
 +	unsigned long flags;
 +
 +	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
 +		vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
 +
 +	spin_lock_irqsave(&pcdev->lock, flags);
 +
 +	buf->state = MX2_STATE_QUEUED;
 +	list_add_tail(&buf->internal.queue, &pcdev->capture);
 +
 +	if (cpu_is_mx25()) {
 +		u32 csicr3, dma_inten = 0;
 +
 +		if (pcdev->fb1_active == NULL) {
 +			writel(vb2_dma_contig_plane_dma_addr(vb, 0),
 +					pcdev->base_csi + CSIDMASA_FB1);
 +			pcdev->fb1_active = buf;
 +			dma_inten = CSICR1_FB1_DMA_INTEN;
 +		} else if (pcdev->fb2_active == NULL) {
 +			writel(vb2_dma_contig_plane_dma_addr(vb, 0),
 +					pcdev->base_csi + CSIDMASA_FB2);
 +			pcdev->fb2_active = buf;
 +			dma_inten = CSICR1_FB2_DMA_INTEN;
 +		}
 +
 +		if (dma_inten) {
 +			list_del(&buf->internal.queue);
 +			buf->state = MX2_STATE_ACTIVE;
 +
 +			csicr3 = readl(pcdev->base_csi + CSICR3);
 +
 +			/* Reflash DMA */
 +			writel(csicr3 | CSICR3_DMA_REFLASH_RFF,
 +					pcdev->base_csi + CSICR3);
 +
 +			/* clear & enable interrupts */
 +			writel(dma_inten, pcdev->base_csi + CSISR);
 +			pcdev->csicr1 |= dma_inten;
 +			writel(pcdev->csicr1, pcdev->base_csi + CSICR1);
 +
 +			/* enable DMA */
 +			csicr3 |= CSICR3_DMA_REQ_EN_RFF | CSICR3_RXFF_LEVEL(1);
 +			writel(csicr3, pcdev->base_csi + CSICR3);
 +		}
 +	}
 +
 +	spin_unlock_irqrestore(&pcdev->lock, flags);
 +}
 +
 +static void mx2_videobuf_release(struct vb2_buffer *vb)
 +{
 +	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx2_camera_dev *pcdev = ici->priv;
 +	struct mx2_buffer *buf = container_of(vb, struct mx2_buffer, vb);
 +	unsigned long flags;
 +
 +#ifdef DEBUG
 +	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
 +		vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
 +
 +	switch (buf->state) {
 +	case MX2_STATE_ACTIVE:
 +		dev_info(icd->parent, "%s (active)\n", __func__);
 +		break;
 +	case MX2_STATE_QUEUED:
 +		dev_info(icd->parent, "%s (queued)\n", __func__);
 +		break;
 +	default:
 +		dev_info(icd->parent, "%s (unknown) %d\n", __func__,
 +				buf->state);
 +		break;
 +	}
 +#endif
 +
 +	/*
 +	 * Terminate only queued but inactive buffers. Active buffers are
 +	 * released when they become inactive after videobuf_waiton().
 +	 *
 +	 * FIXME: implement forced termination of active buffers for mx27 and
 +	 * mx27 eMMA, so that the user won't get stuck in an uninterruptible
 +	 * state. This requires a specific handling for each of the these DMA
 +	 * types.
 +	 */
 +
 +	spin_lock_irqsave(&pcdev->lock, flags);
 +	if (cpu_is_mx25() && buf->state == MX2_STATE_ACTIVE) {
 +		if (pcdev->fb1_active == buf) {
 +			pcdev->csicr1 &= ~CSICR1_FB1_DMA_INTEN;
 +			writel(0, pcdev->base_csi + CSIDMASA_FB1);
 +			pcdev->fb1_active = NULL;
 +		} else if (pcdev->fb2_active == buf) {
 +			pcdev->csicr1 &= ~CSICR1_FB2_DMA_INTEN;
 +			writel(0, pcdev->base_csi + CSIDMASA_FB2);
 +			pcdev->fb2_active = NULL;
 +		}
 +		writel(pcdev->csicr1, pcdev->base_csi + CSICR1);
 +	}
 +	spin_unlock_irqrestore(&pcdev->lock, flags);
 +}
 +
 +static void mx27_camera_emma_buf_init(struct soc_camera_device *icd,
 +		int bytesperline)
 +{
 +	struct soc_camera_host *ici =
 +		to_soc_camera_host(icd->parent);
 +	struct mx2_camera_dev *pcdev = ici->priv;
 +	struct mx2_fmt_cfg *prp = pcdev->emma_prp;
 +
 +	writel((pcdev->s_width << 16) | pcdev->s_height,
 +	       pcdev->base_emma + PRP_SRC_FRAME_SIZE);
 +	writel(prp->cfg.src_pixel,
 +	       pcdev->base_emma + PRP_SRC_PIXEL_FORMAT_CNTL);
 +	if (prp->cfg.channel == 1) {
 +		writel((icd->user_width << 16) | icd->user_height,
 +			pcdev->base_emma + PRP_CH1_OUT_IMAGE_SIZE);
 +		writel(bytesperline,
 +			pcdev->base_emma + PRP_DEST_CH1_LINE_STRIDE);
 +		writel(prp->cfg.ch1_pixel,
 +			pcdev->base_emma + PRP_CH1_PIXEL_FORMAT_CNTL);
 +	} else { /* channel 2 */
 +		writel((icd->user_width << 16) | icd->user_height,
 +			pcdev->base_emma + PRP_CH2_OUT_IMAGE_SIZE);
 +	}
 +
 +	/* Enable interrupts */
 +	writel(prp->cfg.irq_flags, pcdev->base_emma + PRP_INTR_CNTL);
 +}
 +
 +static void mx2_prp_resize_commit(struct mx2_camera_dev *pcdev)
 +{
 +	int dir;
 +
 +	for (dir = RESIZE_DIR_H; dir <= RESIZE_DIR_V; dir++) {
 +		unsigned char *s = pcdev->resizing[dir].s;
 +		int len = pcdev->resizing[dir].len;
 +		unsigned int coeff[2] = {0, 0};
 +		unsigned int valid  = 0;
 +		int i;
 +
 +		if (len == 0)
 +			continue;
 +
 +		for (i = RESIZE_NUM_MAX - 1; i >= 0; i--) {
 +			int j;
 +
 +			j = i > 9 ? 1 : 0;
 +			coeff[j] = (coeff[j] << BC_COEF) |
 +					(s[i] & (SZ_COEF - 1));
 +
 +			if (i == 5 || i == 15)
 +				coeff[j] <<= 1;
 +
 +			valid = (valid << 1) | (s[i] >> BC_COEF);
 +		}
 +
 +		valid |= PRP_RZ_VALID_TBL_LEN(len);
 +
 +		if (pcdev->resizing[dir].algo == RESIZE_ALGO_BILINEAR)
 +			valid |= PRP_RZ_VALID_BILINEAR;
 +
 +		if (pcdev->emma_prp->cfg.channel == 1) {
 +			if (dir == RESIZE_DIR_H) {
 +				writel(coeff[0], pcdev->base_emma +
 +							PRP_CH1_RZ_HORI_COEF1);
 +				writel(coeff[1], pcdev->base_emma +
 +							PRP_CH1_RZ_HORI_COEF2);
 +				writel(valid, pcdev->base_emma +
 +							PRP_CH1_RZ_HORI_VALID);
 +			} else {
 +				writel(coeff[0], pcdev->base_emma +
 +							PRP_CH1_RZ_VERT_COEF1);
 +				writel(coeff[1], pcdev->base_emma +
 +							PRP_CH1_RZ_VERT_COEF2);
 +				writel(valid, pcdev->base_emma +
 +							PRP_CH1_RZ_VERT_VALID);
 +			}
 +		} else {
 +			if (dir == RESIZE_DIR_H) {
 +				writel(coeff[0], pcdev->base_emma +
 +							PRP_CH2_RZ_HORI_COEF1);
 +				writel(coeff[1], pcdev->base_emma +
 +							PRP_CH2_RZ_HORI_COEF2);
 +				writel(valid, pcdev->base_emma +
 +							PRP_CH2_RZ_HORI_VALID);
 +			} else {
 +				writel(coeff[0], pcdev->base_emma +
 +							PRP_CH2_RZ_VERT_COEF1);
 +				writel(coeff[1], pcdev->base_emma +
 +							PRP_CH2_RZ_VERT_COEF2);
 +				writel(valid, pcdev->base_emma +
 +							PRP_CH2_RZ_VERT_VALID);
 +			}
 +		}
 +	}
 +}
 +
 +static int mx2_start_streaming(struct vb2_queue *q, unsigned int count)
 +{
 +	struct soc_camera_device *icd = soc_camera_from_vb2q(q);
 +	struct soc_camera_host *ici =
 +		to_soc_camera_host(icd->parent);
 +	struct mx2_camera_dev *pcdev = ici->priv;
 +	struct mx2_fmt_cfg *prp = pcdev->emma_prp;
 +	struct vb2_buffer *vb;
 +	struct mx2_buffer *buf;
 +	unsigned long phys;
 +	int bytesperline;
 +
 +	if (cpu_is_mx27()) {
 +		unsigned long flags;
 +		if (count < 2)
 +			return -EINVAL;
 +
 +		spin_lock_irqsave(&pcdev->lock, flags);
 +
 +		buf = list_first_entry(&pcdev->capture, struct mx2_buffer,
 +				       internal.queue);
 +		buf->internal.bufnum = 0;
 +		vb = &buf->vb;
 +		buf->state = MX2_STATE_ACTIVE;
 +
 +		phys = vb2_dma_contig_plane_dma_addr(vb, 0);
 +		mx27_update_emma_buf(pcdev, phys, buf->internal.bufnum);
 +		list_move_tail(pcdev->capture.next, &pcdev->active_bufs);
 +
 +		buf = list_first_entry(&pcdev->capture, struct mx2_buffer,
 +				       internal.queue);
 +		buf->internal.bufnum = 1;
 +		vb = &buf->vb;
 +		buf->state = MX2_STATE_ACTIVE;
 +
 +		phys = vb2_dma_contig_plane_dma_addr(vb, 0);
 +		mx27_update_emma_buf(pcdev, phys, buf->internal.bufnum);
 +		list_move_tail(pcdev->capture.next, &pcdev->active_bufs);
 +
 +		bytesperline = soc_mbus_bytes_per_line(icd->user_width,
 +				icd->current_fmt->host_fmt);
 +		if (bytesperline < 0)
 +			return bytesperline;
 +
 +		/*
 +		 * I didn't manage to properly enable/disable the prp
 +		 * on a per frame basis during running transfers,
 +		 * thus we allocate a buffer here and use it to
 +		 * discard frames when no buffer is available.
 +		 * Feel free to work on this ;)
 +		 */
 +		pcdev->discard_size = icd->user_height * bytesperline;
 +		pcdev->discard_buffer = dma_alloc_coherent(ici->v4l2_dev.dev,
 +				pcdev->discard_size, &pcdev->discard_buffer_dma,
 +				GFP_KERNEL);
 +		if (!pcdev->discard_buffer)
 +			return -ENOMEM;
 +
 +		pcdev->buf_discard[0].discard = true;
 +		list_add_tail(&pcdev->buf_discard[0].queue,
 +				      &pcdev->discard);
 +
 +		pcdev->buf_discard[1].discard = true;
 +		list_add_tail(&pcdev->buf_discard[1].queue,
 +				      &pcdev->discard);
 +
 +		mx2_prp_resize_commit(pcdev);
 +
 +		mx27_camera_emma_buf_init(icd, bytesperline);
 +
 +		if (prp->cfg.channel == 1) {
 +			writel(PRP_CNTL_CH1EN |
 +				PRP_CNTL_CSIEN |
 +				prp->cfg.in_fmt |
 +				prp->cfg.out_fmt |
 +				PRP_CNTL_CH1_LEN |
 +				PRP_CNTL_CH1BYP |
 +				PRP_CNTL_CH1_TSKIP(0) |
 +				PRP_CNTL_IN_TSKIP(0),
 +				pcdev->base_emma + PRP_CNTL);
 +		} else {
 +			writel(PRP_CNTL_CH2EN |
 +				PRP_CNTL_CSIEN |
 +				prp->cfg.in_fmt |
 +				prp->cfg.out_fmt |
 +				PRP_CNTL_CH2_LEN |
 +				PRP_CNTL_CH2_TSKIP(0) |
 +				PRP_CNTL_IN_TSKIP(0),
 +				pcdev->base_emma + PRP_CNTL);
 +		}
 +		spin_unlock_irqrestore(&pcdev->lock, flags);
 +	}
 +
 +	return 0;
 +}
 +
 +static int mx2_stop_streaming(struct vb2_queue *q)
 +{
 +	struct soc_camera_device *icd = soc_camera_from_vb2q(q);
 +	struct soc_camera_host *ici =
 +		to_soc_camera_host(icd->parent);
 +	struct mx2_camera_dev *pcdev = ici->priv;
 +	struct mx2_fmt_cfg *prp = pcdev->emma_prp;
 +	unsigned long flags;
 +	void *b;
 +	u32 cntl;
 +
 +	if (cpu_is_mx27()) {
 +		spin_lock_irqsave(&pcdev->lock, flags);
 +
 +		cntl = readl(pcdev->base_emma + PRP_CNTL);
 +		if (prp->cfg.channel == 1) {
 +			writel(cntl & ~PRP_CNTL_CH1EN,
 +			       pcdev->base_emma + PRP_CNTL);
 +		} else {
 +			writel(cntl & ~PRP_CNTL_CH2EN,
 +			       pcdev->base_emma + PRP_CNTL);
 +		}
 +		INIT_LIST_HEAD(&pcdev->capture);
 +		INIT_LIST_HEAD(&pcdev->active_bufs);
 +		INIT_LIST_HEAD(&pcdev->discard);
 +
 +		b = pcdev->discard_buffer;
 +		pcdev->discard_buffer = NULL;
 +
 +		spin_unlock_irqrestore(&pcdev->lock, flags);
 +
 +		dma_free_coherent(ici->v4l2_dev.dev,
 +			pcdev->discard_size, b, pcdev->discard_buffer_dma);
 +	}
 +
 +	return 0;
 +}
 +
 +static struct vb2_ops mx2_videobuf_ops = {
 +	.queue_setup	 = mx2_videobuf_setup,
 +	.buf_prepare	 = mx2_videobuf_prepare,
 +	.buf_queue	 = mx2_videobuf_queue,
 +	.buf_cleanup	 = mx2_videobuf_release,
 +	.start_streaming = mx2_start_streaming,
 +	.stop_streaming	 = mx2_stop_streaming,
 +};
 +
 +static int mx2_camera_init_videobuf(struct vb2_queue *q,
 +			      struct soc_camera_device *icd)
 +{
 +	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 +	q->io_modes = VB2_MMAP | VB2_USERPTR;
 +	q->drv_priv = icd;
 +	q->ops = &mx2_videobuf_ops;
 +	q->mem_ops = &vb2_dma_contig_memops;
 +	q->buf_struct_size = sizeof(struct mx2_buffer);
 +
 +	return vb2_queue_init(q);
 +}
 +
 +#define MX2_BUS_FLAGS	(V4L2_MBUS_MASTER | \
 +			V4L2_MBUS_VSYNC_ACTIVE_HIGH | \
 +			V4L2_MBUS_VSYNC_ACTIVE_LOW | \
 +			V4L2_MBUS_HSYNC_ACTIVE_HIGH | \
 +			V4L2_MBUS_HSYNC_ACTIVE_LOW | \
 +			V4L2_MBUS_PCLK_SAMPLE_RISING | \
 +			V4L2_MBUS_PCLK_SAMPLE_FALLING | \
 +			V4L2_MBUS_DATA_ACTIVE_HIGH | \
 +			V4L2_MBUS_DATA_ACTIVE_LOW)
 +
 +static int mx27_camera_emma_prp_reset(struct mx2_camera_dev *pcdev)
 +{
 +	u32 cntl;
 +	int count = 0;
 +
 +	cntl = readl(pcdev->base_emma + PRP_CNTL);
 +	writel(PRP_CNTL_SWRST, pcdev->base_emma + PRP_CNTL);
 +	while (count++ < 100) {
 +		if (!(readl(pcdev->base_emma + PRP_CNTL) & PRP_CNTL_SWRST))
 +			return 0;
 +		barrier();
 +		udelay(1);
 +	}
 +
 +	return -ETIMEDOUT;
 +}
 +
 +static int mx2_camera_set_bus_param(struct soc_camera_device *icd)
 +{
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx2_camera_dev *pcdev = ici->priv;
 +	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
 +	unsigned long common_flags;
 +	int ret;
 +	int bytesperline;
 +	u32 csicr1 = pcdev->csicr1;
 +
 +	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
 +	if (!ret) {
 +		common_flags = soc_mbus_config_compatible(&cfg, MX2_BUS_FLAGS);
 +		if (!common_flags) {
 +			dev_warn(icd->parent,
 +				 "Flags incompatible: camera 0x%x, host 0x%x\n",
 +				 cfg.flags, MX2_BUS_FLAGS);
 +			return -EINVAL;
 +		}
 +	} else if (ret != -ENOIOCTLCMD) {
 +		return ret;
 +	} else {
 +		common_flags = MX2_BUS_FLAGS;
 +	}
 +
 +	if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
 +	    (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
 +		if (pcdev->platform_flags & MX2_CAMERA_HSYNC_HIGH)
 +			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
 +		else
 +			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
 +	}
 +
 +	if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
 +	    (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
 +		if (pcdev->platform_flags & MX2_CAMERA_PCLK_SAMPLE_RISING)
 +			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
 +		else
 +			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
 +	}
 +
 +	cfg.flags = common_flags;
 +	ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
 +	if (ret < 0 && ret != -ENOIOCTLCMD) {
 +		dev_dbg(icd->parent, "camera s_mbus_config(0x%lx) returned %d\n",
 +			common_flags, ret);
 +		return ret;
 +	}
 +
 +	csicr1 = (csicr1 & ~CSICR1_FMT_MASK) | pcdev->emma_prp->cfg.csicr1;
 +
 +	if (common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
 +		csicr1 |= CSICR1_REDGE;
 +	if (common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
 +		csicr1 |= CSICR1_SOF_POL;
 +	if (common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
 +		csicr1 |= CSICR1_HSYNC_POL;
 +	if (pcdev->platform_flags & MX2_CAMERA_EXT_VSYNC)
 +		csicr1 |= CSICR1_EXT_VSYNC;
 +	if (pcdev->platform_flags & MX2_CAMERA_CCIR)
 +		csicr1 |= CSICR1_CCIR_EN;
 +	if (pcdev->platform_flags & MX2_CAMERA_CCIR_INTERLACE)
 +		csicr1 |= CSICR1_CCIR_MODE;
 +	if (pcdev->platform_flags & MX2_CAMERA_GATED_CLOCK)
 +		csicr1 |= CSICR1_GCLK_MODE;
 +	if (pcdev->platform_flags & MX2_CAMERA_INV_DATA)
 +		csicr1 |= CSICR1_INV_DATA;
 +
 +	pcdev->csicr1 = csicr1;
 +
 +	bytesperline = soc_mbus_bytes_per_line(icd->user_width,
 +			icd->current_fmt->host_fmt);
 +	if (bytesperline < 0)
 +		return bytesperline;
 +
 +	if (cpu_is_mx27()) {
 +		ret = mx27_camera_emma_prp_reset(pcdev);
 +		if (ret)
 +			return ret;
 +	} else if (cpu_is_mx25()) {
 +		writel((bytesperline * icd->user_height) >> 2,
 +				pcdev->base_csi + CSIRXCNT);
 +		writel((bytesperline << 16) | icd->user_height,
 +				pcdev->base_csi + CSIIMAG_PARA);
 +	}
 +
 +	writel(pcdev->csicr1, pcdev->base_csi + CSICR1);
 +
 +	return 0;
 +}
 +
 +static int mx2_camera_set_crop(struct soc_camera_device *icd,
 +				struct v4l2_crop *a)
 +{
 +	struct v4l2_rect *rect = &a->c;
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	struct v4l2_mbus_framefmt mf;
 +	int ret;
 +
 +	soc_camera_limit_side(&rect->left, &rect->width, 0, 2, 4096);
 +	soc_camera_limit_side(&rect->top, &rect->height, 0, 2, 4096);
 +
 +	ret = v4l2_subdev_call(sd, video, s_crop, a);
 +	if (ret < 0)
 +		return ret;
 +
 +	/* The capture device might have changed its output  */
 +	ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
 +	if (ret < 0)
 +		return ret;
 +
 +	dev_dbg(icd->parent, "Sensor cropped %dx%d\n",
 +		mf.width, mf.height);
 +
 +	icd->user_width		= mf.width;
 +	icd->user_height	= mf.height;
 +
 +	return ret;
 +}
 +
 +static int mx2_camera_get_formats(struct soc_camera_device *icd,
 +				  unsigned int idx,
 +				  struct soc_camera_format_xlate *xlate)
 +{
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	const struct soc_mbus_pixelfmt *fmt;
 +	struct device *dev = icd->parent;
 +	enum v4l2_mbus_pixelcode code;
 +	int ret, formats = 0;
 +
 +	ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
 +	if (ret < 0)
 +		/* no more formats */
 +		return 0;
 +
 +	fmt = soc_mbus_get_fmtdesc(code);
 +	if (!fmt) {
 +		dev_err(dev, "Invalid format code #%u: %d\n", idx, code);
 +		return 0;
 +	}
 +
 +	if (code == V4L2_MBUS_FMT_YUYV8_2X8 ||
 +	    code == V4L2_MBUS_FMT_UYVY8_2X8) {
 +		formats++;
 +		if (xlate) {
 +			/*
 +			 * CH2 can output YUV420 which is a standard format in
 +			 * soc_mediabus.c
 +			 */
 +			xlate->host_fmt =
 +				soc_mbus_get_fmtdesc(V4L2_MBUS_FMT_YUYV8_1_5X8);
 +			xlate->code	= code;
 +			dev_dbg(dev, "Providing host format %s for sensor code %d\n",
 +			       xlate->host_fmt->name, code);
 +			xlate++;
 +		}
 +	}
 +
 +	if (code == V4L2_MBUS_FMT_UYVY8_2X8) {
 +		formats++;
 +		if (xlate) {
 +			xlate->host_fmt =
 +				soc_mbus_get_fmtdesc(V4L2_MBUS_FMT_YUYV8_2X8);
 +			xlate->code	= code;
 +			dev_dbg(dev, "Providing host format %s for sensor code %d\n",
 +				xlate->host_fmt->name, code);
 +			xlate++;
 +		}
 +	}
 +
 +	/* Generic pass-trough */
 +	formats++;
 +	if (xlate) {
 +		xlate->host_fmt = fmt;
 +		xlate->code	= code;
 +		xlate++;
 +	}
 +	return formats;
 +}
 +
 +static int mx2_emmaprp_resize(struct mx2_camera_dev *pcdev,
 +			      struct v4l2_mbus_framefmt *mf_in,
 +			      struct v4l2_pix_format *pix_out, bool apply)
 +{
 +	int num, den;
 +	unsigned long m;
 +	int i, dir;
 +
 +	for (dir = RESIZE_DIR_H; dir <= RESIZE_DIR_V; dir++) {
 +		struct emma_prp_resize tmprsz;
 +		unsigned char *s = tmprsz.s;
 +		int len = 0;
 +		int in, out;
 +
 +		if (dir == RESIZE_DIR_H) {
 +			in = mf_in->width;
 +			out = pix_out->width;
 +		} else {
 +			in = mf_in->height;
 +			out = pix_out->height;
 +		}
 +
 +		if (in < out)
 +			return -EINVAL;
 +		else if (in == out)
 +			continue;
 +
 +		/* Calculate ratio */
 +		m = gcd(in, out);
 +		num = in / m;
 +		den = out / m;
 +		if (num > RESIZE_NUM_MAX)
 +			return -EINVAL;
 +
 +		if ((num >= 2 * den) && (den == 1) &&
 +		    (num < 9) && (!(num & 0x01))) {
 +			int sum = 0;
 +			int j;
 +
 +			/* Average scaling for >= 2:1 ratios */
 +			/* Support can be added for num >=9 and odd values */
 +
 +			tmprsz.algo = RESIZE_ALGO_AVERAGING;
 +			len = num;
 +
 +			for (i = 0; i < (len / 2); i++)
 +				s[i] = 8;
 +
 +			do {
 +				for (i = 0; i < (len / 2); i++) {
 +					s[i] = s[i] >> 1;
 +					sum = 0;
 +					for (j = 0; j < (len / 2); j++)
 +						sum += s[j];
 +					if (sum == 4)
 +						break;
 +				}
 +			} while (sum != 4);
 +
 +			for (i = (len / 2); i < len; i++)
 +				s[i] = s[len - i - 1];
 +
 +			s[len - 1] |= SZ_COEF;
 +		} else {
 +			/* bilinear scaling for < 2:1 ratios */
 +			int v; /* overflow counter */
 +			int coeff, nxt; /* table output */
 +			int in_pos_inc = 2 * den;
 +			int out_pos = num;
 +			int out_pos_inc = 2 * num;
 +			int init_carry = num - den;
 +			int carry = init_carry;
 +
 +			tmprsz.algo = RESIZE_ALGO_BILINEAR;
 +			v = den + in_pos_inc;
 +			do {
 +				coeff = v - out_pos;
 +				out_pos += out_pos_inc;
 +				carry += out_pos_inc;
 +				for (nxt = 0; v < out_pos; nxt++) {
 +					v += in_pos_inc;
 +					carry -= in_pos_inc;
 +				}
 +
 +				if (len > RESIZE_NUM_MAX)
 +					return -EINVAL;
 +
 +				coeff = ((coeff << BC_COEF) +
 +					(in_pos_inc >> 1)) / in_pos_inc;
 +
 +				if (coeff >= (SZ_COEF - 1))
 +					coeff--;
 +
 +				coeff |= SZ_COEF;
 +				s[len] = (unsigned char)coeff;
 +				len++;
 +
 +				for (i = 1; i < nxt; i++) {
 +					if (len >= RESIZE_NUM_MAX)
 +						return -EINVAL;
 +					s[len] = 0;
 +					len++;
 +				}
 +			} while (carry != init_carry);
 +		}
 +		tmprsz.len = len;
 +		if (dir == RESIZE_DIR_H)
 +			mf_in->width = pix_out->width;
 +		else
 +			mf_in->height = pix_out->height;
 +
 +		if (apply)
 +			memcpy(&pcdev->resizing[dir], &tmprsz, sizeof(tmprsz));
 +	}
 +	return 0;
 +}
 +
 +static int mx2_camera_set_fmt(struct soc_camera_device *icd,
 +			       struct v4l2_format *f)
 +{
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx2_camera_dev *pcdev = ici->priv;
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	const struct soc_camera_format_xlate *xlate;
 +	struct v4l2_pix_format *pix = &f->fmt.pix;
 +	struct v4l2_mbus_framefmt mf;
 +	int ret;
 +
 +	dev_dbg(icd->parent, "%s: requested params: width = %d, height = %d\n",
 +		__func__, pix->width, pix->height);
 +
 +	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
 +	if (!xlate) {
 +		dev_warn(icd->parent, "Format %x not found\n",
 +				pix->pixelformat);
 +		return -EINVAL;
 +	}
 +
 +	mf.width	= pix->width;
 +	mf.height	= pix->height;
 +	mf.field	= pix->field;
 +	mf.colorspace	= pix->colorspace;
 +	mf.code		= xlate->code;
 +
 +	ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
 +	if (ret < 0 && ret != -ENOIOCTLCMD)
 +		return ret;
 +
 +	/* Store width and height returned by the sensor for resizing */
 +	pcdev->s_width = mf.width;
 +	pcdev->s_height = mf.height;
 +	dev_dbg(icd->parent, "%s: sensor params: width = %d, height = %d\n",
 +		__func__, pcdev->s_width, pcdev->s_height);
 +
 +	pcdev->emma_prp = mx27_emma_prp_get_format(xlate->code,
 +						   xlate->host_fmt->fourcc);
 +
 +	memset(pcdev->resizing, 0, sizeof(pcdev->resizing));
 +	if ((mf.width != pix->width || mf.height != pix->height) &&
 +		pcdev->emma_prp->cfg.in_fmt == PRP_CNTL_DATA_IN_YUV422) {
 +		if (mx2_emmaprp_resize(pcdev, &mf, pix, true) < 0)
 +			dev_dbg(icd->parent, "%s: can't resize\n", __func__);
 +	}
 +
 +	if (mf.code != xlate->code)
 +		return -EINVAL;
 +
 +	pix->width		= mf.width;
 +	pix->height		= mf.height;
 +	pix->field		= mf.field;
 +	pix->colorspace		= mf.colorspace;
 +	icd->current_fmt	= xlate;
 +
 +	dev_dbg(icd->parent, "%s: returned params: width = %d, height = %d\n",
 +		__func__, pix->width, pix->height);
 +
 +	return 0;
 +}
 +
 +static int mx2_camera_try_fmt(struct soc_camera_device *icd,
 +				  struct v4l2_format *f)
 +{
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	const struct soc_camera_format_xlate *xlate;
 +	struct v4l2_pix_format *pix = &f->fmt.pix;
 +	struct v4l2_mbus_framefmt mf;
 +	__u32 pixfmt = pix->pixelformat;
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx2_camera_dev *pcdev = ici->priv;
 +	unsigned int width_limit;
 +	int ret;
 +
 +	dev_dbg(icd->parent, "%s: requested params: width = %d, height = %d\n",
 +		__func__, pix->width, pix->height);
 +
 +	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
 +	if (pixfmt && !xlate) {
 +		dev_warn(icd->parent, "Format %x not found\n", pixfmt);
 +		return -EINVAL;
 +	}
 +
 +	/* FIXME: implement MX27 limits */
 +
 +	/* limit to MX25 hardware capabilities */
 +	if (cpu_is_mx25()) {
 +		if (xlate->host_fmt->bits_per_sample <= 8)
 +			width_limit = 0xffff * 4;
 +		else
 +			width_limit = 0xffff * 2;
 +		/* CSIIMAG_PARA limit */
 +		if (pix->width > width_limit)
 +			pix->width = width_limit;
 +		if (pix->height > 0xffff)
 +			pix->height = 0xffff;
 +
 +		pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
 +				xlate->host_fmt);
 +		if (pix->bytesperline < 0)
 +			return pix->bytesperline;
 +		pix->sizeimage = soc_mbus_image_size(xlate->host_fmt,
 +						pix->bytesperline, pix->height);
 +		/* Check against the CSIRXCNT limit */
 +		if (pix->sizeimage > 4 * 0x3ffff) {
 +			/* Adjust geometry, preserve aspect ratio */
 +			unsigned int new_height = int_sqrt(div_u64(0x3ffffULL *
 +					4 * pix->height, pix->bytesperline));
 +			pix->width = new_height * pix->width / pix->height;
 +			pix->height = new_height;
 +			pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
 +							xlate->host_fmt);
 +			BUG_ON(pix->bytesperline < 0);
 +			pix->sizeimage = soc_mbus_image_size(xlate->host_fmt,
 +						pix->bytesperline, pix->height);
 +		}
 +	}
 +
 +	/* limit to sensor capabilities */
 +	mf.width	= pix->width;
 +	mf.height	= pix->height;
 +	mf.field	= pix->field;
 +	mf.colorspace	= pix->colorspace;
 +	mf.code		= xlate->code;
 +
 +	ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
 +	if (ret < 0)
 +		return ret;
 +
 +	dev_dbg(icd->parent, "%s: sensor params: width = %d, height = %d\n",
 +		__func__, pcdev->s_width, pcdev->s_height);
 +
 +	/* If the sensor does not support image size try PrP resizing */
 +	pcdev->emma_prp = mx27_emma_prp_get_format(xlate->code,
 +						   xlate->host_fmt->fourcc);
 +
 +	memset(pcdev->resizing, 0, sizeof(pcdev->resizing));
 +	if ((mf.width != pix->width || mf.height != pix->height) &&
 +		pcdev->emma_prp->cfg.in_fmt == PRP_CNTL_DATA_IN_YUV422) {
 +		if (mx2_emmaprp_resize(pcdev, &mf, pix, false) < 0)
 +			dev_dbg(icd->parent, "%s: can't resize\n", __func__);
 +	}
 +
 +	if (mf.field == V4L2_FIELD_ANY)
 +		mf.field = V4L2_FIELD_NONE;
 +	/*
 +	 * Driver supports interlaced images provided they have
 +	 * both fields so that they can be processed as if they
 +	 * were progressive.
 +	 */
 +	if (mf.field != V4L2_FIELD_NONE && !V4L2_FIELD_HAS_BOTH(mf.field)) {
 +		dev_err(icd->parent, "Field type %d unsupported.\n",
 +				mf.field);
 +		return -EINVAL;
 +	}
 +
 +	pix->width	= mf.width;
 +	pix->height	= mf.height;
 +	pix->field	= mf.field;
 +	pix->colorspace	= mf.colorspace;
 +
 +	dev_dbg(icd->parent, "%s: returned params: width = %d, height = %d\n",
 +		__func__, pix->width, pix->height);
 +
 +	return 0;
 +}
 +
 +static int mx2_camera_querycap(struct soc_camera_host *ici,
 +			       struct v4l2_capability *cap)
 +{
 +	/* cap->name is set by the friendly caller:-> */
 +	strlcpy(cap->card, MX2_CAM_DRIVER_DESCRIPTION, sizeof(cap->card));
 +	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 +
 +	return 0;
 +}
 +
 +static unsigned int mx2_camera_poll(struct file *file, poll_table *pt)
 +{
 +	struct soc_camera_device *icd = file->private_data;
 +
 +	return vb2_poll(&icd->vb2_vidq, file, pt);
 +}
 +
 +static struct soc_camera_host_ops mx2_soc_camera_host_ops = {
 +	.owner		= THIS_MODULE,
 +	.add		= mx2_camera_add_device,
 +	.remove		= mx2_camera_remove_device,
 +	.set_fmt	= mx2_camera_set_fmt,
 +	.set_crop	= mx2_camera_set_crop,
 +	.get_formats	= mx2_camera_get_formats,
 +	.try_fmt	= mx2_camera_try_fmt,
 +	.init_videobuf2	= mx2_camera_init_videobuf,
 +	.poll		= mx2_camera_poll,
 +	.querycap	= mx2_camera_querycap,
 +	.set_bus_param	= mx2_camera_set_bus_param,
 +};
 +
 +static void mx27_camera_frame_done_emma(struct mx2_camera_dev *pcdev,
 +		int bufnum, bool err)
 +{
 +#ifdef DEBUG
 +	struct mx2_fmt_cfg *prp = pcdev->emma_prp;
 +#endif
 +	struct mx2_buf_internal *ibuf;
 +	struct mx2_buffer *buf;
 +	struct vb2_buffer *vb;
 +	unsigned long phys;
 +
 +	ibuf = list_first_entry(&pcdev->active_bufs, struct mx2_buf_internal,
 +			       queue);
 +
 +	BUG_ON(ibuf->bufnum != bufnum);
 +
 +	if (ibuf->discard) {
 +		/*
 +		 * Discard buffer must not be returned to user space.
 +		 * Just return it to the discard queue.
 +		 */
 +		list_move_tail(pcdev->active_bufs.next, &pcdev->discard);
 +	} else {
 +		buf = mx2_ibuf_to_buf(ibuf);
 +
 +		vb = &buf->vb;
 +#ifdef DEBUG
 +		phys = vb2_dma_contig_plane_dma_addr(vb, 0);
 +		if (prp->cfg.channel == 1) {
 +			if (readl(pcdev->base_emma + PRP_DEST_RGB1_PTR +
 +				4 * bufnum) != phys) {
 +				dev_err(pcdev->dev, "%lx != %x\n", phys,
 +					readl(pcdev->base_emma +
 +					PRP_DEST_RGB1_PTR + 4 * bufnum));
 +			}
 +		} else {
 +			if (readl(pcdev->base_emma + PRP_DEST_Y_PTR -
 +				0x14 * bufnum) != phys) {
 +				dev_err(pcdev->dev, "%lx != %x\n", phys,
 +					readl(pcdev->base_emma +
 +					PRP_DEST_Y_PTR - 0x14 * bufnum));
 +			}
 +		}
 +#endif
 +		dev_dbg(pcdev->dev, "%s (vb=0x%p) 0x%p %lu\n", __func__, vb,
 +				vb2_plane_vaddr(vb, 0),
 +				vb2_get_plane_payload(vb, 0));
 +
 +		list_del_init(&buf->internal.queue);
 +		do_gettimeofday(&vb->v4l2_buf.timestamp);
 +		vb->v4l2_buf.sequence = pcdev->frame_count;
 +		if (err)
 +			vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
 +		else
 +			vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
 +	}
 +
 +	pcdev->frame_count++;
 +
 +	if (list_empty(&pcdev->capture)) {
 +		if (list_empty(&pcdev->discard)) {
 +			dev_warn(pcdev->dev, "%s: trying to access empty discard list\n",
 +				 __func__);
 +			return;
 +		}
 +
 +		ibuf = list_first_entry(&pcdev->discard,
 +					struct mx2_buf_internal, queue);
 +		ibuf->bufnum = bufnum;
 +
 +		list_move_tail(pcdev->discard.next, &pcdev->active_bufs);
 +		mx27_update_emma_buf(pcdev, pcdev->discard_buffer_dma, bufnum);
 +		return;
 +	}
 +
 +	buf = list_first_entry(&pcdev->capture, struct mx2_buffer,
 +			       internal.queue);
 +
 +	buf->internal.bufnum = bufnum;
 +
 +	list_move_tail(pcdev->capture.next, &pcdev->active_bufs);
 +
 +	vb = &buf->vb;
 +	buf->state = MX2_STATE_ACTIVE;
 +
 +	phys = vb2_dma_contig_plane_dma_addr(vb, 0);
 +	mx27_update_emma_buf(pcdev, phys, bufnum);
 +}
 +
 +static irqreturn_t mx27_camera_emma_irq(int irq_emma, void *data)
 +{
 +	struct mx2_camera_dev *pcdev = data;
 +	unsigned int status = readl(pcdev->base_emma + PRP_INTRSTATUS);
 +	struct mx2_buf_internal *ibuf;
 +
 +	spin_lock(&pcdev->lock);
 +
 +	if (list_empty(&pcdev->active_bufs)) {
 +		dev_warn(pcdev->dev, "%s: called while active list is empty\n",
 +			__func__);
 +
 +		if (!status) {
 +			spin_unlock(&pcdev->lock);
 +			return IRQ_NONE;
 +		}
 +	}
 +
 +	if (status & (1 << 7)) { /* overflow */
 +		u32 cntl = readl(pcdev->base_emma + PRP_CNTL);
 +		writel(cntl & ~(PRP_CNTL_CH1EN | PRP_CNTL_CH2EN),
 +		       pcdev->base_emma + PRP_CNTL);
 +		writel(cntl, pcdev->base_emma + PRP_CNTL);
 +
 +		ibuf = list_first_entry(&pcdev->active_bufs,
 +					struct mx2_buf_internal, queue);
 +		mx27_camera_frame_done_emma(pcdev,
 +					ibuf->bufnum, true);
 +
 +		status &= ~(1 << 7);
 +	} else if (((status & (3 << 5)) == (3 << 5)) ||
 +		((status & (3 << 3)) == (3 << 3))) {
 +		/*
 +		 * Both buffers have triggered, process the one we're expecting
 +		 * to first
 +		 */
 +		ibuf = list_first_entry(&pcdev->active_bufs,
 +					struct mx2_buf_internal, queue);
 +		mx27_camera_frame_done_emma(pcdev, ibuf->bufnum, false);
 +		status &= ~(1 << (6 - ibuf->bufnum)); /* mark processed */
 +	} else if ((status & (1 << 6)) || (status & (1 << 4))) {
 +		mx27_camera_frame_done_emma(pcdev, 0, false);
 +	} else if ((status & (1 << 5)) || (status & (1 << 3))) {
 +		mx27_camera_frame_done_emma(pcdev, 1, false);
 +	}
 +
 +	spin_unlock(&pcdev->lock);
 +	writel(status, pcdev->base_emma + PRP_INTRSTATUS);
 +
 +	return IRQ_HANDLED;
 +}
 +
 +static int __devinit mx27_camera_emma_init(struct platform_device *pdev)
 +{
 +	struct mx2_camera_dev *pcdev = platform_get_drvdata(pdev);
 +	struct resource *res_emma;
 +	int irq_emma;
 +	int err = 0;
 +
 +	res_emma = platform_get_resource(pdev, IORESOURCE_MEM, 1);
 +	irq_emma = platform_get_irq(pdev, 1);
 +	if (!res_emma || !irq_emma) {
 +		dev_err(pcdev->dev, "no EMMA resources\n");
 +		goto out;
 +	}
 +
 +	pcdev->base_emma = devm_request_and_ioremap(pcdev->dev, res_emma);
 +	if (!pcdev->base_emma) {
 +		err = -EADDRNOTAVAIL;
 +		goto out;
 +	}
 +
 +	err = devm_request_irq(pcdev->dev, irq_emma, mx27_camera_emma_irq, 0,
 +			       MX2_CAM_DRV_NAME, pcdev);
 +	if (err) {
 +		dev_err(pcdev->dev, "Camera EMMA interrupt register failed \n");
 +		goto out;
 +	}
 +
 +	pcdev->clk_emma_ipg = devm_clk_get(pcdev->dev, "emma-ipg");
 +	if (IS_ERR(pcdev->clk_emma_ipg)) {
 +		err = PTR_ERR(pcdev->clk_emma_ipg);
 +		goto out;
 +	}
 +
 +	clk_prepare_enable(pcdev->clk_emma_ipg);
 +
 +	pcdev->clk_emma_ahb = devm_clk_get(pcdev->dev, "emma-ahb");
 +	if (IS_ERR(pcdev->clk_emma_ahb)) {
 +		err = PTR_ERR(pcdev->clk_emma_ahb);
 +		goto exit_clk_emma_ipg;
 +	}
 +
 +	clk_prepare_enable(pcdev->clk_emma_ahb);
 +
 +	err = mx27_camera_emma_prp_reset(pcdev);
 +	if (err)
 +		goto exit_clk_emma_ahb;
 +
 +	return err;
 +
 +exit_clk_emma_ahb:
 +	clk_disable_unprepare(pcdev->clk_emma_ahb);
 +exit_clk_emma_ipg:
 +	clk_disable_unprepare(pcdev->clk_emma_ipg);
 +out:
 +	return err;
 +}
 +
 +static int __devinit mx2_camera_probe(struct platform_device *pdev)
 +{
 +	struct mx2_camera_dev *pcdev;
 +	struct resource *res_csi;
 +	int irq_csi;
 +	int err = 0;
 +
 +	dev_dbg(&pdev->dev, "initialising\n");
 +
 +	res_csi = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 +	irq_csi = platform_get_irq(pdev, 0);
 +	if (res_csi == NULL || irq_csi < 0) {
 +		dev_err(&pdev->dev, "Missing platform resources data\n");
 +		err = -ENODEV;
 +		goto exit;
 +	}
 +
 +	pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
 +	if (!pcdev) {
 +		dev_err(&pdev->dev, "Could not allocate pcdev\n");
 +		err = -ENOMEM;
 +		goto exit;
 +	}
 +
 +	pcdev->clk_csi = devm_clk_get(&pdev->dev, "ahb");
 +	if (IS_ERR(pcdev->clk_csi)) {
 +		dev_err(&pdev->dev, "Could not get csi clock\n");
 +		err = PTR_ERR(pcdev->clk_csi);
 +		goto exit;
 +	}
 +
 +	pcdev->pdata = pdev->dev.platform_data;
 +	if (pcdev->pdata) {
 +		long rate;
 +
 +		pcdev->platform_flags = pcdev->pdata->flags;
 +
 +		rate = clk_round_rate(pcdev->clk_csi, pcdev->pdata->clk * 2);
 +		if (rate <= 0) {
 +			err = -ENODEV;
 +			goto exit;
 +		}
 +		err = clk_set_rate(pcdev->clk_csi, rate);
 +		if (err < 0)
 +			goto exit;
 +	}
 +
 +	INIT_LIST_HEAD(&pcdev->capture);
 +	INIT_LIST_HEAD(&pcdev->active_bufs);
 +	INIT_LIST_HEAD(&pcdev->discard);
 +	spin_lock_init(&pcdev->lock);
 +
 +	pcdev->base_csi = devm_request_and_ioremap(&pdev->dev, res_csi);
 +	if (!pcdev->base_csi) {
 +		err = -EADDRNOTAVAIL;
 +		goto exit;
 +	}
 +
 +	pcdev->dev = &pdev->dev;
 +	platform_set_drvdata(pdev, pcdev);
 +
 +	if (cpu_is_mx25()) {
 +		err = devm_request_irq(&pdev->dev, irq_csi, mx25_camera_irq, 0,
 +				       MX2_CAM_DRV_NAME, pcdev);
 +		if (err) {
 +			dev_err(pcdev->dev, "Camera interrupt register failed \n");
 +			goto exit;
 +		}
 +	}
 +
 +	if (cpu_is_mx27()) {
 +		err = mx27_camera_emma_init(pdev);
 +		if (err)
 +			goto exit;
 +	}
 +
 +	/*
 +	 * We're done with drvdata here.  Clear the pointer so that
 +	 * v4l2 core can start using drvdata on its purpose.
 +	 */
 +	platform_set_drvdata(pdev, NULL);
 +
 +	pcdev->soc_host.drv_name	= MX2_CAM_DRV_NAME,
 +	pcdev->soc_host.ops		= &mx2_soc_camera_host_ops,
 +	pcdev->soc_host.priv		= pcdev;
 +	pcdev->soc_host.v4l2_dev.dev	= &pdev->dev;
 +	pcdev->soc_host.nr		= pdev->id;
 +	if (cpu_is_mx25())
 +		pcdev->soc_host.capabilities = SOCAM_HOST_CAP_STRIDE;
 +
 +	pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
 +	if (IS_ERR(pcdev->alloc_ctx)) {
 +		err = PTR_ERR(pcdev->alloc_ctx);
 +		goto eallocctx;
 +	}
 +	err = soc_camera_host_register(&pcdev->soc_host);
 +	if (err)
 +		goto exit_free_emma;
 +
 +	dev_info(&pdev->dev, "MX2 Camera (CSI) driver probed, clock frequency: %ld\n",
 +			clk_get_rate(pcdev->clk_csi));
 +
 +	return 0;
 +
 +exit_free_emma:
 +	vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
 +eallocctx:
 +	if (cpu_is_mx27()) {
 +		clk_disable_unprepare(pcdev->clk_emma_ipg);
 +		clk_disable_unprepare(pcdev->clk_emma_ahb);
 +	}
 +exit:
 +	return err;
 +}
 +
 +static int __devexit mx2_camera_remove(struct platform_device *pdev)
 +{
 +	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
 +	struct mx2_camera_dev *pcdev = container_of(soc_host,
 +			struct mx2_camera_dev, soc_host);
 +
 +	soc_camera_host_unregister(&pcdev->soc_host);
 +
 +	vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
 +
 +	if (cpu_is_mx27()) {
 +		clk_disable_unprepare(pcdev->clk_emma_ipg);
 +		clk_disable_unprepare(pcdev->clk_emma_ahb);
 +	}
 +
 +	dev_info(&pdev->dev, "MX2 Camera driver unloaded\n");
 +
 +	return 0;
 +}
 +
 +static struct platform_driver mx2_camera_driver = {
 +	.driver 	= {
 +		.name	= MX2_CAM_DRV_NAME,
 +	},
 +	.remove		= __devexit_p(mx2_camera_remove),
 +};
 +
 +
 +static int __init mx2_camera_init(void)
 +{
 +	return platform_driver_probe(&mx2_camera_driver, &mx2_camera_probe);
 +}
 +
 +static void __exit mx2_camera_exit(void)
 +{
 +	return platform_driver_unregister(&mx2_camera_driver);
 +}
 +
 +module_init(mx2_camera_init);
 +module_exit(mx2_camera_exit);
 +
 +MODULE_DESCRIPTION("i.MX27/i.MX25 SoC Camera Host driver");
 +MODULE_AUTHOR("Sascha Hauer <sha@pengutronix.de>");
 +MODULE_LICENSE("GPL");
 +MODULE_VERSION(MX2_CAM_VERSION);
diff --cc drivers/media/platform/soc_camera/mx3_camera.c
index 16975c6e6905,000000000000..3557ac97e430
mode 100644,000000..100644
--- a/drivers/media/platform/soc_camera/mx3_camera.c
+++ b/drivers/media/platform/soc_camera/mx3_camera.c
@@@ -1,1290 -1,0 +1,1290 @@@
 +/*
 + * V4L2 Driver for i.MX3x camera host
 + *
 + * Copyright (C) 2008
 + * Guennadi Liakhovetski, DENX Software Engineering, <lg@denx.de>
 + *
 + * This program is free software; you can redistribute it and/or modify
 + * it under the terms of the GNU General Public License version 2 as
 + * published by the Free Software Foundation.
 + */
 +
 +#include <linux/init.h>
 +#include <linux/module.h>
 +#include <linux/videodev2.h>
 +#include <linux/platform_device.h>
 +#include <linux/clk.h>
 +#include <linux/vmalloc.h>
 +#include <linux/interrupt.h>
 +#include <linux/sched.h>
 +
 +#include <media/v4l2-common.h>
 +#include <media/v4l2-dev.h>
 +#include <media/videobuf2-dma-contig.h>
 +#include <media/soc_camera.h>
 +#include <media/soc_mediabus.h>
 +
 +#include <mach/ipu.h>
- #include <mach/mx3_camera.h>
- #include <mach/dma.h>
++#include <linux/platform_data/camera-mx3.h>
++#include <linux/platform_data/dma-imx.h>
 +
 +#define MX3_CAM_DRV_NAME "mx3-camera"
 +
 +/* CMOS Sensor Interface Registers */
 +#define CSI_REG_START		0x60
 +
 +#define CSI_SENS_CONF		(0x60 - CSI_REG_START)
 +#define CSI_SENS_FRM_SIZE	(0x64 - CSI_REG_START)
 +#define CSI_ACT_FRM_SIZE	(0x68 - CSI_REG_START)
 +#define CSI_OUT_FRM_CTRL	(0x6C - CSI_REG_START)
 +#define CSI_TST_CTRL		(0x70 - CSI_REG_START)
 +#define CSI_CCIR_CODE_1		(0x74 - CSI_REG_START)
 +#define CSI_CCIR_CODE_2		(0x78 - CSI_REG_START)
 +#define CSI_CCIR_CODE_3		(0x7C - CSI_REG_START)
 +#define CSI_FLASH_STROBE_1	(0x80 - CSI_REG_START)
 +#define CSI_FLASH_STROBE_2	(0x84 - CSI_REG_START)
 +
 +#define CSI_SENS_CONF_VSYNC_POL_SHIFT		0
 +#define CSI_SENS_CONF_HSYNC_POL_SHIFT		1
 +#define CSI_SENS_CONF_DATA_POL_SHIFT		2
 +#define CSI_SENS_CONF_PIX_CLK_POL_SHIFT		3
 +#define CSI_SENS_CONF_SENS_PRTCL_SHIFT		4
 +#define CSI_SENS_CONF_SENS_CLKSRC_SHIFT		7
 +#define CSI_SENS_CONF_DATA_FMT_SHIFT		8
 +#define CSI_SENS_CONF_DATA_WIDTH_SHIFT		10
 +#define CSI_SENS_CONF_EXT_VSYNC_SHIFT		15
 +#define CSI_SENS_CONF_DIVRATIO_SHIFT		16
 +
 +#define CSI_SENS_CONF_DATA_FMT_RGB_YUV444	(0UL << CSI_SENS_CONF_DATA_FMT_SHIFT)
 +#define CSI_SENS_CONF_DATA_FMT_YUV422		(2UL << CSI_SENS_CONF_DATA_FMT_SHIFT)
 +#define CSI_SENS_CONF_DATA_FMT_BAYER		(3UL << CSI_SENS_CONF_DATA_FMT_SHIFT)
 +
 +#define MAX_VIDEO_MEM 16
 +
 +struct mx3_camera_buffer {
 +	/* common v4l buffer stuff -- must be first */
 +	struct vb2_buffer			vb;
 +	struct list_head			queue;
 +
 +	/* One descriptot per scatterlist (per frame) */
 +	struct dma_async_tx_descriptor		*txd;
 +
 +	/* We have to "build" a scatterlist ourselves - one element per frame */
 +	struct scatterlist			sg;
 +};
 +
 +/**
 + * struct mx3_camera_dev - i.MX3x camera (CSI) object
 + * @dev:		camera device, to which the coherent buffer is attached
 + * @icd:		currently attached camera sensor
 + * @clk:		pointer to clock
 + * @base:		remapped register base address
 + * @pdata:		platform data
 + * @platform_flags:	platform flags
 + * @mclk:		master clock frequency in Hz
 + * @capture:		list of capture videobuffers
 + * @lock:		protects video buffer lists
 + * @active:		active video buffer
 + * @idmac_channel:	array of pointers to IPU DMAC DMA channels
 + * @soc_host:		embedded soc_host object
 + */
 +struct mx3_camera_dev {
 +	/*
 +	 * i.MX3x is only supposed to handle one camera on its Camera Sensor
 +	 * Interface. If anyone ever builds hardware to enable more than one
 +	 * camera _simultaneously_, they will have to modify this driver too
 +	 */
 +	struct soc_camera_device *icd;
 +	struct clk		*clk;
 +
 +	void __iomem		*base;
 +
 +	struct mx3_camera_pdata	*pdata;
 +
 +	unsigned long		platform_flags;
 +	unsigned long		mclk;
 +	u16			width_flags;	/* max 15 bits */
 +
 +	struct list_head	capture;
 +	spinlock_t		lock;		/* Protects video buffer lists */
 +	struct mx3_camera_buffer *active;
 +	size_t			buf_total;
 +	struct vb2_alloc_ctx	*alloc_ctx;
 +	enum v4l2_field		field;
 +	int			sequence;
 +
 +	/* IDMAC / dmaengine interface */
 +	struct idmac_channel	*idmac_channel[1];	/* We need one channel */
 +
 +	struct soc_camera_host	soc_host;
 +};
 +
 +struct dma_chan_request {
 +	struct mx3_camera_dev	*mx3_cam;
 +	enum ipu_channel	id;
 +};
 +
 +static u32 csi_reg_read(struct mx3_camera_dev *mx3, off_t reg)
 +{
 +	return __raw_readl(mx3->base + reg);
 +}
 +
 +static void csi_reg_write(struct mx3_camera_dev *mx3, u32 value, off_t reg)
 +{
 +	__raw_writel(value, mx3->base + reg);
 +}
 +
 +static struct mx3_camera_buffer *to_mx3_vb(struct vb2_buffer *vb)
 +{
 +	return container_of(vb, struct mx3_camera_buffer, vb);
 +}
 +
 +/* Called from the IPU IDMAC ISR */
 +static void mx3_cam_dma_done(void *arg)
 +{
 +	struct idmac_tx_desc *desc = to_tx_desc(arg);
 +	struct dma_chan *chan = desc->txd.chan;
 +	struct idmac_channel *ichannel = to_idmac_chan(chan);
 +	struct mx3_camera_dev *mx3_cam = ichannel->client;
 +
 +	dev_dbg(chan->device->dev, "callback cookie %d, active DMA 0x%08x\n",
 +		desc->txd.cookie, mx3_cam->active ? sg_dma_address(&mx3_cam->active->sg) : 0);
 +
 +	spin_lock(&mx3_cam->lock);
 +	if (mx3_cam->active) {
 +		struct vb2_buffer *vb = &mx3_cam->active->vb;
 +		struct mx3_camera_buffer *buf = to_mx3_vb(vb);
 +
 +		list_del_init(&buf->queue);
 +		do_gettimeofday(&vb->v4l2_buf.timestamp);
 +		vb->v4l2_buf.field = mx3_cam->field;
 +		vb->v4l2_buf.sequence = mx3_cam->sequence++;
 +		vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
 +	}
 +
 +	if (list_empty(&mx3_cam->capture)) {
 +		mx3_cam->active = NULL;
 +		spin_unlock(&mx3_cam->lock);
 +
 +		/*
 +		 * stop capture - without further buffers IPU_CHA_BUF0_RDY will
 +		 * not get updated
 +		 */
 +		return;
 +	}
 +
 +	mx3_cam->active = list_entry(mx3_cam->capture.next,
 +				     struct mx3_camera_buffer, queue);
 +	spin_unlock(&mx3_cam->lock);
 +}
 +
 +/*
 + * Videobuf operations
 + */
 +
 +/*
 + * Calculate the __buffer__ (not data) size and number of buffers.
 + */
 +static int mx3_videobuf_setup(struct vb2_queue *vq,
 +			const struct v4l2_format *fmt,
 +			unsigned int *count, unsigned int *num_planes,
 +			unsigned int sizes[], void *alloc_ctxs[])
 +{
 +	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx3_camera_dev *mx3_cam = ici->priv;
 +
 +	if (!mx3_cam->idmac_channel[0])
 +		return -EINVAL;
 +
 +	if (fmt) {
 +		const struct soc_camera_format_xlate *xlate = soc_camera_xlate_by_fourcc(icd,
 +								fmt->fmt.pix.pixelformat);
 +		unsigned int bytes_per_line;
 +		int ret;
 +
 +		if (!xlate)
 +			return -EINVAL;
 +
 +		ret = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
 +					      xlate->host_fmt);
 +		if (ret < 0)
 +			return ret;
 +
 +		bytes_per_line = max_t(u32, fmt->fmt.pix.bytesperline, ret);
 +
 +		ret = soc_mbus_image_size(xlate->host_fmt, bytes_per_line,
 +					  fmt->fmt.pix.height);
 +		if (ret < 0)
 +			return ret;
 +
 +		sizes[0] = max_t(u32, fmt->fmt.pix.sizeimage, ret);
 +	} else {
 +		/* Called from VIDIOC_REQBUFS or in compatibility mode */
 +		sizes[0] = icd->sizeimage;
 +	}
 +
 +	alloc_ctxs[0] = mx3_cam->alloc_ctx;
 +
 +	if (!vq->num_buffers)
 +		mx3_cam->sequence = 0;
 +
 +	if (!*count)
 +		*count = 2;
 +
 +	/* If *num_planes != 0, we have already verified *count. */
 +	if (!*num_planes &&
 +	    sizes[0] * *count + mx3_cam->buf_total > MAX_VIDEO_MEM * 1024 * 1024)
 +		*count = (MAX_VIDEO_MEM * 1024 * 1024 - mx3_cam->buf_total) /
 +			sizes[0];
 +
 +	*num_planes = 1;
 +
 +	return 0;
 +}
 +
 +static enum pixel_fmt fourcc_to_ipu_pix(__u32 fourcc)
 +{
 +	/* Add more formats as need arises and test possibilities appear... */
 +	switch (fourcc) {
 +	case V4L2_PIX_FMT_RGB24:
 +		return IPU_PIX_FMT_RGB24;
 +	case V4L2_PIX_FMT_UYVY:
 +	case V4L2_PIX_FMT_RGB565:
 +	default:
 +		return IPU_PIX_FMT_GENERIC;
 +	}
 +}
 +
 +static void mx3_videobuf_queue(struct vb2_buffer *vb)
 +{
 +	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx3_camera_dev *mx3_cam = ici->priv;
 +	struct mx3_camera_buffer *buf = to_mx3_vb(vb);
 +	struct scatterlist *sg = &buf->sg;
 +	struct dma_async_tx_descriptor *txd;
 +	struct idmac_channel *ichan = mx3_cam->idmac_channel[0];
 +	struct idmac_video_param *video = &ichan->params.video;
 +	const struct soc_mbus_pixelfmt *host_fmt = icd->current_fmt->host_fmt;
 +	unsigned long flags;
 +	dma_cookie_t cookie;
 +	size_t new_size;
 +
 +	new_size = icd->sizeimage;
 +
 +	if (vb2_plane_size(vb, 0) < new_size) {
 +		dev_err(icd->parent, "Buffer #%d too small (%lu < %zu)\n",
 +			vb->v4l2_buf.index, vb2_plane_size(vb, 0), new_size);
 +		goto error;
 +	}
 +
 +	if (!buf->txd) {
 +		sg_dma_address(sg)	= vb2_dma_contig_plane_dma_addr(vb, 0);
 +		sg_dma_len(sg)		= new_size;
 +
 +		txd = dmaengine_prep_slave_sg(
 +			&ichan->dma_chan, sg, 1, DMA_DEV_TO_MEM,
 +			DMA_PREP_INTERRUPT);
 +		if (!txd)
 +			goto error;
 +
 +		txd->callback_param	= txd;
 +		txd->callback		= mx3_cam_dma_done;
 +
 +		buf->txd		= txd;
 +	} else {
 +		txd = buf->txd;
 +	}
 +
 +	vb2_set_plane_payload(vb, 0, new_size);
 +
 +	/* This is the configuration of one sg-element */
 +	video->out_pixel_fmt = fourcc_to_ipu_pix(host_fmt->fourcc);
 +
 +	if (video->out_pixel_fmt == IPU_PIX_FMT_GENERIC) {
 +		/*
 +		 * If the IPU DMA channel is configured to transfer generic
 +		 * 8-bit data, we have to set up the geometry parameters
 +		 * correctly, according to the current pixel format. The DMA
 +		 * horizontal parameters in this case are expressed in bytes,
 +		 * not in pixels.
 +		 */
 +		video->out_width	= icd->bytesperline;
 +		video->out_height	= icd->user_height;
 +		video->out_stride	= icd->bytesperline;
 +	} else {
 +		/*
 +		 * For IPU known formats the pixel unit will be managed
 +		 * successfully by the IPU code
 +		 */
 +		video->out_width	= icd->user_width;
 +		video->out_height	= icd->user_height;
 +		video->out_stride	= icd->user_width;
 +	}
 +
 +#ifdef DEBUG
 +	/* helps to see what DMA actually has written */
 +	if (vb2_plane_vaddr(vb, 0))
 +		memset(vb2_plane_vaddr(vb, 0), 0xaa, vb2_get_plane_payload(vb, 0));
 +#endif
 +
 +	spin_lock_irqsave(&mx3_cam->lock, flags);
 +	list_add_tail(&buf->queue, &mx3_cam->capture);
 +
 +	if (!mx3_cam->active)
 +		mx3_cam->active = buf;
 +
 +	spin_unlock_irq(&mx3_cam->lock);
 +
 +	cookie = txd->tx_submit(txd);
 +	dev_dbg(icd->parent, "Submitted cookie %d DMA 0x%08x\n",
 +		cookie, sg_dma_address(&buf->sg));
 +
 +	if (cookie >= 0)
 +		return;
 +
 +	spin_lock_irq(&mx3_cam->lock);
 +
 +	/* Submit error */
 +	list_del_init(&buf->queue);
 +
 +	if (mx3_cam->active == buf)
 +		mx3_cam->active = NULL;
 +
 +	spin_unlock_irqrestore(&mx3_cam->lock, flags);
 +error:
 +	vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
 +}
 +
 +static void mx3_videobuf_release(struct vb2_buffer *vb)
 +{
 +	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx3_camera_dev *mx3_cam = ici->priv;
 +	struct mx3_camera_buffer *buf = to_mx3_vb(vb);
 +	struct dma_async_tx_descriptor *txd = buf->txd;
 +	unsigned long flags;
 +
 +	dev_dbg(icd->parent,
 +		"Release%s DMA 0x%08x, queue %sempty\n",
 +		mx3_cam->active == buf ? " active" : "", sg_dma_address(&buf->sg),
 +		list_empty(&buf->queue) ? "" : "not ");
 +
 +	spin_lock_irqsave(&mx3_cam->lock, flags);
 +
 +	if (mx3_cam->active == buf)
 +		mx3_cam->active = NULL;
 +
 +	/* Doesn't hurt also if the list is empty */
 +	list_del_init(&buf->queue);
 +
 +	if (txd) {
 +		buf->txd = NULL;
 +		if (mx3_cam->idmac_channel[0])
 +			async_tx_ack(txd);
 +	}
 +
 +	spin_unlock_irqrestore(&mx3_cam->lock, flags);
 +
 +	mx3_cam->buf_total -= vb2_plane_size(vb, 0);
 +}
 +
 +static int mx3_videobuf_init(struct vb2_buffer *vb)
 +{
 +	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx3_camera_dev *mx3_cam = ici->priv;
 +	struct mx3_camera_buffer *buf = to_mx3_vb(vb);
 +
 +	if (!buf->txd) {
 +		/* This is for locking debugging only */
 +		INIT_LIST_HEAD(&buf->queue);
 +		sg_init_table(&buf->sg, 1);
 +
 +		mx3_cam->buf_total += vb2_plane_size(vb, 0);
 +	}
 +
 +	return 0;
 +}
 +
 +static int mx3_stop_streaming(struct vb2_queue *q)
 +{
 +	struct soc_camera_device *icd = soc_camera_from_vb2q(q);
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx3_camera_dev *mx3_cam = ici->priv;
 +	struct idmac_channel *ichan = mx3_cam->idmac_channel[0];
 +	struct mx3_camera_buffer *buf, *tmp;
 +	unsigned long flags;
 +
 +	if (ichan) {
 +		struct dma_chan *chan = &ichan->dma_chan;
 +		chan->device->device_control(chan, DMA_PAUSE, 0);
 +	}
 +
 +	spin_lock_irqsave(&mx3_cam->lock, flags);
 +
 +	mx3_cam->active = NULL;
 +
 +	list_for_each_entry_safe(buf, tmp, &mx3_cam->capture, queue) {
 +		list_del_init(&buf->queue);
 +		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
 +	}
 +
 +	spin_unlock_irqrestore(&mx3_cam->lock, flags);
 +
 +	return 0;
 +}
 +
 +static struct vb2_ops mx3_videobuf_ops = {
 +	.queue_setup	= mx3_videobuf_setup,
 +	.buf_queue	= mx3_videobuf_queue,
 +	.buf_cleanup	= mx3_videobuf_release,
 +	.buf_init	= mx3_videobuf_init,
 +	.wait_prepare	= soc_camera_unlock,
 +	.wait_finish	= soc_camera_lock,
 +	.stop_streaming	= mx3_stop_streaming,
 +};
 +
 +static int mx3_camera_init_videobuf(struct vb2_queue *q,
 +				     struct soc_camera_device *icd)
 +{
 +	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 +	q->io_modes = VB2_MMAP | VB2_USERPTR;
 +	q->drv_priv = icd;
 +	q->ops = &mx3_videobuf_ops;
 +	q->mem_ops = &vb2_dma_contig_memops;
 +	q->buf_struct_size = sizeof(struct mx3_camera_buffer);
 +
 +	return vb2_queue_init(q);
 +}
 +
 +/* First part of ipu_csi_init_interface() */
 +static void mx3_camera_activate(struct mx3_camera_dev *mx3_cam,
 +				struct soc_camera_device *icd)
 +{
 +	u32 conf;
 +	long rate;
 +
 +	/* Set default size: ipu_csi_set_window_size() */
 +	csi_reg_write(mx3_cam, (640 - 1) | ((480 - 1) << 16), CSI_ACT_FRM_SIZE);
 +	/* ...and position to 0:0: ipu_csi_set_window_pos() */
 +	conf = csi_reg_read(mx3_cam, CSI_OUT_FRM_CTRL) & 0xffff0000;
 +	csi_reg_write(mx3_cam, conf, CSI_OUT_FRM_CTRL);
 +
 +	/* We use only gated clock synchronisation mode so far */
 +	conf = 0 << CSI_SENS_CONF_SENS_PRTCL_SHIFT;
 +
 +	/* Set generic data, platform-biggest bus-width */
 +	conf |= CSI_SENS_CONF_DATA_FMT_BAYER;
 +
 +	if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_15)
 +		conf |= 3 << CSI_SENS_CONF_DATA_WIDTH_SHIFT;
 +	else if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_10)
 +		conf |= 2 << CSI_SENS_CONF_DATA_WIDTH_SHIFT;
 +	else if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_8)
 +		conf |= 1 << CSI_SENS_CONF_DATA_WIDTH_SHIFT;
 +	else/* if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_4)*/
 +		conf |= 0 << CSI_SENS_CONF_DATA_WIDTH_SHIFT;
 +
 +	if (mx3_cam->platform_flags & MX3_CAMERA_CLK_SRC)
 +		conf |= 1 << CSI_SENS_CONF_SENS_CLKSRC_SHIFT;
 +	if (mx3_cam->platform_flags & MX3_CAMERA_EXT_VSYNC)
 +		conf |= 1 << CSI_SENS_CONF_EXT_VSYNC_SHIFT;
 +	if (mx3_cam->platform_flags & MX3_CAMERA_DP)
 +		conf |= 1 << CSI_SENS_CONF_DATA_POL_SHIFT;
 +	if (mx3_cam->platform_flags & MX3_CAMERA_PCP)
 +		conf |= 1 << CSI_SENS_CONF_PIX_CLK_POL_SHIFT;
 +	if (mx3_cam->platform_flags & MX3_CAMERA_HSP)
 +		conf |= 1 << CSI_SENS_CONF_HSYNC_POL_SHIFT;
 +	if (mx3_cam->platform_flags & MX3_CAMERA_VSP)
 +		conf |= 1 << CSI_SENS_CONF_VSYNC_POL_SHIFT;
 +
 +	/* ipu_csi_init_interface() */
 +	csi_reg_write(mx3_cam, conf, CSI_SENS_CONF);
 +
 +	clk_prepare_enable(mx3_cam->clk);
 +	rate = clk_round_rate(mx3_cam->clk, mx3_cam->mclk);
 +	dev_dbg(icd->parent, "Set SENS_CONF to %x, rate %ld\n", conf, rate);
 +	if (rate)
 +		clk_set_rate(mx3_cam->clk, rate);
 +}
 +
 +/* Called with .video_lock held */
 +static int mx3_camera_add_device(struct soc_camera_device *icd)
 +{
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx3_camera_dev *mx3_cam = ici->priv;
 +
 +	if (mx3_cam->icd)
 +		return -EBUSY;
 +
 +	mx3_camera_activate(mx3_cam, icd);
 +
 +	mx3_cam->buf_total = 0;
 +	mx3_cam->icd = icd;
 +
 +	dev_info(icd->parent, "MX3 Camera driver attached to camera %d\n",
 +		 icd->devnum);
 +
 +	return 0;
 +}
 +
 +/* Called with .video_lock held */
 +static void mx3_camera_remove_device(struct soc_camera_device *icd)
 +{
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx3_camera_dev *mx3_cam = ici->priv;
 +	struct idmac_channel **ichan = &mx3_cam->idmac_channel[0];
 +
 +	BUG_ON(icd != mx3_cam->icd);
 +
 +	if (*ichan) {
 +		dma_release_channel(&(*ichan)->dma_chan);
 +		*ichan = NULL;
 +	}
 +
 +	clk_disable_unprepare(mx3_cam->clk);
 +
 +	mx3_cam->icd = NULL;
 +
 +	dev_info(icd->parent, "MX3 Camera driver detached from camera %d\n",
 +		 icd->devnum);
 +}
 +
 +static int test_platform_param(struct mx3_camera_dev *mx3_cam,
 +			       unsigned char buswidth, unsigned long *flags)
 +{
 +	/*
 +	 * If requested data width is supported by the platform, use it or any
 +	 * possible lower value - i.MX31 is smart enough to shift bits
 +	 */
 +	if (buswidth > fls(mx3_cam->width_flags))
 +		return -EINVAL;
 +
 +	/*
 +	 * Platform specified synchronization and pixel clock polarities are
 +	 * only a recommendation and are only used during probing. MX3x
 +	 * camera interface only works in master mode, i.e., uses HSYNC and
 +	 * VSYNC signals from the sensor
 +	 */
 +	*flags = V4L2_MBUS_MASTER |
 +		V4L2_MBUS_HSYNC_ACTIVE_HIGH |
 +		V4L2_MBUS_HSYNC_ACTIVE_LOW |
 +		V4L2_MBUS_VSYNC_ACTIVE_HIGH |
 +		V4L2_MBUS_VSYNC_ACTIVE_LOW |
 +		V4L2_MBUS_PCLK_SAMPLE_RISING |
 +		V4L2_MBUS_PCLK_SAMPLE_FALLING |
 +		V4L2_MBUS_DATA_ACTIVE_HIGH |
 +		V4L2_MBUS_DATA_ACTIVE_LOW;
 +
 +	return 0;
 +}
 +
 +static int mx3_camera_try_bus_param(struct soc_camera_device *icd,
 +				    const unsigned int depth)
 +{
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx3_camera_dev *mx3_cam = ici->priv;
 +	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
 +	unsigned long bus_flags, common_flags;
 +	int ret = test_platform_param(mx3_cam, depth, &bus_flags);
 +
 +	dev_dbg(icd->parent, "request bus width %d bit: %d\n", depth, ret);
 +
 +	if (ret < 0)
 +		return ret;
 +
 +	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
 +	if (!ret) {
 +		common_flags = soc_mbus_config_compatible(&cfg,
 +							  bus_flags);
 +		if (!common_flags) {
 +			dev_warn(icd->parent,
 +				 "Flags incompatible: camera 0x%x, host 0x%lx\n",
 +				 cfg.flags, bus_flags);
 +			return -EINVAL;
 +		}
 +	} else if (ret != -ENOIOCTLCMD) {
 +		return ret;
 +	}
 +
 +	return 0;
 +}
 +
 +static bool chan_filter(struct dma_chan *chan, void *arg)
 +{
 +	struct dma_chan_request *rq = arg;
 +	struct mx3_camera_pdata *pdata;
 +
 +	if (!imx_dma_is_ipu(chan))
 +		return false;
 +
 +	if (!rq)
 +		return false;
 +
 +	pdata = rq->mx3_cam->soc_host.v4l2_dev.dev->platform_data;
 +
 +	return rq->id == chan->chan_id &&
 +		pdata->dma_dev == chan->device->dev;
 +}
 +
 +static const struct soc_mbus_pixelfmt mx3_camera_formats[] = {
 +	{
 +		.fourcc			= V4L2_PIX_FMT_SBGGR8,
 +		.name			= "Bayer BGGR (sRGB) 8 bit",
 +		.bits_per_sample	= 8,
 +		.packing		= SOC_MBUS_PACKING_NONE,
 +		.order			= SOC_MBUS_ORDER_LE,
 +		.layout			= SOC_MBUS_LAYOUT_PACKED,
 +	}, {
 +		.fourcc			= V4L2_PIX_FMT_GREY,
 +		.name			= "Monochrome 8 bit",
 +		.bits_per_sample	= 8,
 +		.packing		= SOC_MBUS_PACKING_NONE,
 +		.order			= SOC_MBUS_ORDER_LE,
 +		.layout			= SOC_MBUS_LAYOUT_PACKED,
 +	},
 +};
 +
 +/* This will be corrected as we get more formats */
 +static bool mx3_camera_packing_supported(const struct soc_mbus_pixelfmt *fmt)
 +{
 +	return	fmt->packing == SOC_MBUS_PACKING_NONE ||
 +		(fmt->bits_per_sample == 8 &&
 +		 fmt->packing == SOC_MBUS_PACKING_2X8_PADHI) ||
 +		(fmt->bits_per_sample > 8 &&
 +		 fmt->packing == SOC_MBUS_PACKING_EXTEND16);
 +}
 +
 +static int mx3_camera_get_formats(struct soc_camera_device *icd, unsigned int idx,
 +				  struct soc_camera_format_xlate *xlate)
 +{
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	struct device *dev = icd->parent;
 +	int formats = 0, ret;
 +	enum v4l2_mbus_pixelcode code;
 +	const struct soc_mbus_pixelfmt *fmt;
 +
 +	ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
 +	if (ret < 0)
 +		/* No more formats */
 +		return 0;
 +
 +	fmt = soc_mbus_get_fmtdesc(code);
 +	if (!fmt) {
 +		dev_warn(icd->parent,
 +			 "Unsupported format code #%u: %d\n", idx, code);
 +		return 0;
 +	}
 +
 +	/* This also checks support for the requested bits-per-sample */
 +	ret = mx3_camera_try_bus_param(icd, fmt->bits_per_sample);
 +	if (ret < 0)
 +		return 0;
 +
 +	switch (code) {
 +	case V4L2_MBUS_FMT_SBGGR10_1X10:
 +		formats++;
 +		if (xlate) {
 +			xlate->host_fmt	= &mx3_camera_formats[0];
 +			xlate->code	= code;
 +			xlate++;
 +			dev_dbg(dev, "Providing format %s using code %d\n",
 +				mx3_camera_formats[0].name, code);
 +		}
 +		break;
 +	case V4L2_MBUS_FMT_Y10_1X10:
 +		formats++;
 +		if (xlate) {
 +			xlate->host_fmt	= &mx3_camera_formats[1];
 +			xlate->code	= code;
 +			xlate++;
 +			dev_dbg(dev, "Providing format %s using code %d\n",
 +				mx3_camera_formats[1].name, code);
 +		}
 +		break;
 +	default:
 +		if (!mx3_camera_packing_supported(fmt))
 +			return 0;
 +	}
 +
 +	/* Generic pass-through */
 +	formats++;
 +	if (xlate) {
 +		xlate->host_fmt	= fmt;
 +		xlate->code	= code;
 +		dev_dbg(dev, "Providing format %c%c%c%c in pass-through mode\n",
 +			(fmt->fourcc >> (0*8)) & 0xFF,
 +			(fmt->fourcc >> (1*8)) & 0xFF,
 +			(fmt->fourcc >> (2*8)) & 0xFF,
 +			(fmt->fourcc >> (3*8)) & 0xFF);
 +		xlate++;
 +	}
 +
 +	return formats;
 +}
 +
 +static void configure_geometry(struct mx3_camera_dev *mx3_cam,
 +			       unsigned int width, unsigned int height,
 +			       const struct soc_mbus_pixelfmt *fmt)
 +{
 +	u32 ctrl, width_field, height_field;
 +
 +	if (fourcc_to_ipu_pix(fmt->fourcc) == IPU_PIX_FMT_GENERIC) {
 +		/*
 +		 * As the CSI will be configured to output BAYER, here
 +		 * the width parameter count the number of samples to
 +		 * capture to complete the whole image width.
 +		 */
 +		unsigned int num, den;
 +		int ret = soc_mbus_samples_per_pixel(fmt, &num, &den);
 +		BUG_ON(ret < 0);
 +		width = width * num / den;
 +	}
 +
 +	/* Setup frame size - this cannot be changed on-the-fly... */
 +	width_field = width - 1;
 +	height_field = height - 1;
 +	csi_reg_write(mx3_cam, width_field | (height_field << 16), CSI_SENS_FRM_SIZE);
 +
 +	csi_reg_write(mx3_cam, width_field << 16, CSI_FLASH_STROBE_1);
 +	csi_reg_write(mx3_cam, (height_field << 16) | 0x22, CSI_FLASH_STROBE_2);
 +
 +	csi_reg_write(mx3_cam, width_field | (height_field << 16), CSI_ACT_FRM_SIZE);
 +
 +	/* ...and position */
 +	ctrl = csi_reg_read(mx3_cam, CSI_OUT_FRM_CTRL) & 0xffff0000;
 +	/* Sensor does the cropping */
 +	csi_reg_write(mx3_cam, ctrl | 0 | (0 << 8), CSI_OUT_FRM_CTRL);
 +}
 +
 +static int acquire_dma_channel(struct mx3_camera_dev *mx3_cam)
 +{
 +	dma_cap_mask_t mask;
 +	struct dma_chan *chan;
 +	struct idmac_channel **ichan = &mx3_cam->idmac_channel[0];
 +	/* We have to use IDMAC_IC_7 for Bayer / generic data */
 +	struct dma_chan_request rq = {.mx3_cam = mx3_cam,
 +				      .id = IDMAC_IC_7};
 +
 +	dma_cap_zero(mask);
 +	dma_cap_set(DMA_SLAVE, mask);
 +	dma_cap_set(DMA_PRIVATE, mask);
 +	chan = dma_request_channel(mask, chan_filter, &rq);
 +	if (!chan)
 +		return -EBUSY;
 +
 +	*ichan = to_idmac_chan(chan);
 +	(*ichan)->client = mx3_cam;
 +
 +	return 0;
 +}
 +
 +/*
 + * FIXME: learn to use stride != width, then we can keep stride properly aligned
 + * and support arbitrary (even) widths.
 + */
 +static inline void stride_align(__u32 *width)
 +{
 +	if (ALIGN(*width, 8) < 4096)
 +		*width = ALIGN(*width, 8);
 +	else
 +		*width = *width &  ~7;
 +}
 +
 +/*
 + * As long as we don't implement host-side cropping and scaling, we can use
 + * default g_crop and cropcap from soc_camera.c
 + */
 +static int mx3_camera_set_crop(struct soc_camera_device *icd,
 +			       struct v4l2_crop *a)
 +{
 +	struct v4l2_rect *rect = &a->c;
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx3_camera_dev *mx3_cam = ici->priv;
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	struct v4l2_mbus_framefmt mf;
 +	int ret;
 +
 +	soc_camera_limit_side(&rect->left, &rect->width, 0, 2, 4096);
 +	soc_camera_limit_side(&rect->top, &rect->height, 0, 2, 4096);
 +
 +	ret = v4l2_subdev_call(sd, video, s_crop, a);
 +	if (ret < 0)
 +		return ret;
 +
 +	/* The capture device might have changed its output sizes */
 +	ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
 +	if (ret < 0)
 +		return ret;
 +
 +	if (mf.code != icd->current_fmt->code)
 +		return -EINVAL;
 +
 +	if (mf.width & 7) {
 +		/* Ouch! We can only handle 8-byte aligned width... */
 +		stride_align(&mf.width);
 +		ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
 +		if (ret < 0)
 +			return ret;
 +	}
 +
 +	if (mf.width != icd->user_width || mf.height != icd->user_height)
 +		configure_geometry(mx3_cam, mf.width, mf.height,
 +				   icd->current_fmt->host_fmt);
 +
 +	dev_dbg(icd->parent, "Sensor cropped %dx%d\n",
 +		mf.width, mf.height);
 +
 +	icd->user_width		= mf.width;
 +	icd->user_height	= mf.height;
 +
 +	return ret;
 +}
 +
 +static int mx3_camera_set_fmt(struct soc_camera_device *icd,
 +			      struct v4l2_format *f)
 +{
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx3_camera_dev *mx3_cam = ici->priv;
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	const struct soc_camera_format_xlate *xlate;
 +	struct v4l2_pix_format *pix = &f->fmt.pix;
 +	struct v4l2_mbus_framefmt mf;
 +	int ret;
 +
 +	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
 +	if (!xlate) {
 +		dev_warn(icd->parent, "Format %x not found\n",
 +			 pix->pixelformat);
 +		return -EINVAL;
 +	}
 +
 +	stride_align(&pix->width);
 +	dev_dbg(icd->parent, "Set format %dx%d\n", pix->width, pix->height);
 +
 +	/*
 +	 * Might have to perform a complete interface initialisation like in
 +	 * ipu_csi_init_interface() in mxc_v4l2_s_param(). Also consider
 +	 * mxc_v4l2_s_fmt()
 +	 */
 +
 +	configure_geometry(mx3_cam, pix->width, pix->height, xlate->host_fmt);
 +
 +	mf.width	= pix->width;
 +	mf.height	= pix->height;
 +	mf.field	= pix->field;
 +	mf.colorspace	= pix->colorspace;
 +	mf.code		= xlate->code;
 +
 +	ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
 +	if (ret < 0)
 +		return ret;
 +
 +	if (mf.code != xlate->code)
 +		return -EINVAL;
 +
 +	if (!mx3_cam->idmac_channel[0]) {
 +		ret = acquire_dma_channel(mx3_cam);
 +		if (ret < 0)
 +			return ret;
 +	}
 +
 +	pix->width		= mf.width;
 +	pix->height		= mf.height;
 +	pix->field		= mf.field;
 +	mx3_cam->field		= mf.field;
 +	pix->colorspace		= mf.colorspace;
 +	icd->current_fmt	= xlate;
 +
 +	dev_dbg(icd->parent, "Sensor set %dx%d\n", pix->width, pix->height);
 +
 +	return ret;
 +}
 +
 +static int mx3_camera_try_fmt(struct soc_camera_device *icd,
 +			      struct v4l2_format *f)
 +{
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	const struct soc_camera_format_xlate *xlate;
 +	struct v4l2_pix_format *pix = &f->fmt.pix;
 +	struct v4l2_mbus_framefmt mf;
 +	__u32 pixfmt = pix->pixelformat;
 +	int ret;
 +
 +	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
 +	if (pixfmt && !xlate) {
 +		dev_warn(icd->parent, "Format %x not found\n", pixfmt);
 +		return -EINVAL;
 +	}
 +
 +	/* limit to MX3 hardware capabilities */
 +	if (pix->height > 4096)
 +		pix->height = 4096;
 +	if (pix->width > 4096)
 +		pix->width = 4096;
 +
 +	/* limit to sensor capabilities */
 +	mf.width	= pix->width;
 +	mf.height	= pix->height;
 +	mf.field	= pix->field;
 +	mf.colorspace	= pix->colorspace;
 +	mf.code		= xlate->code;
 +
 +	ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
 +	if (ret < 0)
 +		return ret;
 +
 +	pix->width	= mf.width;
 +	pix->height	= mf.height;
 +	pix->colorspace	= mf.colorspace;
 +
 +	switch (mf.field) {
 +	case V4L2_FIELD_ANY:
 +		pix->field = V4L2_FIELD_NONE;
 +		break;
 +	case V4L2_FIELD_NONE:
 +		break;
 +	default:
 +		dev_err(icd->parent, "Field type %d unsupported.\n",
 +			mf.field);
 +		ret = -EINVAL;
 +	}
 +
 +	return ret;
 +}
 +
 +static int mx3_camera_reqbufs(struct soc_camera_device *icd,
 +			      struct v4l2_requestbuffers *p)
 +{
 +	return 0;
 +}
 +
 +static unsigned int mx3_camera_poll(struct file *file, poll_table *pt)
 +{
 +	struct soc_camera_device *icd = file->private_data;
 +
 +	return vb2_poll(&icd->vb2_vidq, file, pt);
 +}
 +
 +static int mx3_camera_querycap(struct soc_camera_host *ici,
 +			       struct v4l2_capability *cap)
 +{
 +	/* cap->name is set by the firendly caller:-> */
 +	strlcpy(cap->card, "i.MX3x Camera", sizeof(cap->card));
 +	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 +
 +	return 0;
 +}
 +
 +static int mx3_camera_set_bus_param(struct soc_camera_device *icd)
 +{
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct mx3_camera_dev *mx3_cam = ici->priv;
 +	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
 +	u32 pixfmt = icd->current_fmt->host_fmt->fourcc;
 +	unsigned long bus_flags, common_flags;
 +	u32 dw, sens_conf;
 +	const struct soc_mbus_pixelfmt *fmt;
 +	int buswidth;
 +	int ret;
 +	const struct soc_camera_format_xlate *xlate;
 +	struct device *dev = icd->parent;
 +
 +	fmt = soc_mbus_get_fmtdesc(icd->current_fmt->code);
 +	if (!fmt)
 +		return -EINVAL;
 +
 +	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
 +	if (!xlate) {
 +		dev_warn(dev, "Format %x not found\n", pixfmt);
 +		return -EINVAL;
 +	}
 +
 +	buswidth = fmt->bits_per_sample;
 +	ret = test_platform_param(mx3_cam, buswidth, &bus_flags);
 +
 +	dev_dbg(dev, "requested bus width %d bit: %d\n", buswidth, ret);
 +
 +	if (ret < 0)
 +		return ret;
 +
 +	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
 +	if (!ret) {
 +		common_flags = soc_mbus_config_compatible(&cfg,
 +							  bus_flags);
 +		if (!common_flags) {
 +			dev_warn(icd->parent,
 +				 "Flags incompatible: camera 0x%x, host 0x%lx\n",
 +				 cfg.flags, bus_flags);
 +			return -EINVAL;
 +		}
 +	} else if (ret != -ENOIOCTLCMD) {
 +		return ret;
 +	} else {
 +		common_flags = bus_flags;
 +	}
 +
 +	dev_dbg(dev, "Flags cam: 0x%x host: 0x%lx common: 0x%lx\n",
 +		cfg.flags, bus_flags, common_flags);
 +
 +	/* Make choices, based on platform preferences */
 +	if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
 +	    (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
 +		if (mx3_cam->platform_flags & MX3_CAMERA_HSP)
 +			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
 +		else
 +			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
 +	}
 +
 +	if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
 +	    (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
 +		if (mx3_cam->platform_flags & MX3_CAMERA_VSP)
 +			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
 +		else
 +			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
 +	}
 +
 +	if ((common_flags & V4L2_MBUS_DATA_ACTIVE_HIGH) &&
 +	    (common_flags & V4L2_MBUS_DATA_ACTIVE_LOW)) {
 +		if (mx3_cam->platform_flags & MX3_CAMERA_DP)
 +			common_flags &= ~V4L2_MBUS_DATA_ACTIVE_HIGH;
 +		else
 +			common_flags &= ~V4L2_MBUS_DATA_ACTIVE_LOW;
 +	}
 +
 +	if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
 +	    (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
 +		if (mx3_cam->platform_flags & MX3_CAMERA_PCP)
 +			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
 +		else
 +			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
 +	}
 +
 +	cfg.flags = common_flags;
 +	ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
 +	if (ret < 0 && ret != -ENOIOCTLCMD) {
 +		dev_dbg(dev, "camera s_mbus_config(0x%lx) returned %d\n",
 +			common_flags, ret);
 +		return ret;
 +	}
 +
 +	/*
 +	 * So far only gated clock mode is supported. Add a line
 +	 *	(3 << CSI_SENS_CONF_SENS_PRTCL_SHIFT) |
 +	 * below and select the required mode when supporting other
 +	 * synchronisation protocols.
 +	 */
 +	sens_conf = csi_reg_read(mx3_cam, CSI_SENS_CONF) &
 +		~((1 << CSI_SENS_CONF_VSYNC_POL_SHIFT) |
 +		  (1 << CSI_SENS_CONF_HSYNC_POL_SHIFT) |
 +		  (1 << CSI_SENS_CONF_DATA_POL_SHIFT) |
 +		  (1 << CSI_SENS_CONF_PIX_CLK_POL_SHIFT) |
 +		  (3 << CSI_SENS_CONF_DATA_FMT_SHIFT) |
 +		  (3 << CSI_SENS_CONF_DATA_WIDTH_SHIFT));
 +
 +	/* TODO: Support RGB and YUV formats */
 +
 +	/* This has been set in mx3_camera_activate(), but we clear it above */
 +	sens_conf |= CSI_SENS_CONF_DATA_FMT_BAYER;
 +
 +	if (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
 +		sens_conf |= 1 << CSI_SENS_CONF_PIX_CLK_POL_SHIFT;
 +	if (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
 +		sens_conf |= 1 << CSI_SENS_CONF_HSYNC_POL_SHIFT;
 +	if (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
 +		sens_conf |= 1 << CSI_SENS_CONF_VSYNC_POL_SHIFT;
 +	if (common_flags & V4L2_MBUS_DATA_ACTIVE_LOW)
 +		sens_conf |= 1 << CSI_SENS_CONF_DATA_POL_SHIFT;
 +
 +	/* Just do what we're asked to do */
 +	switch (xlate->host_fmt->bits_per_sample) {
 +	case 4:
 +		dw = 0 << CSI_SENS_CONF_DATA_WIDTH_SHIFT;
 +		break;
 +	case 8:
 +		dw = 1 << CSI_SENS_CONF_DATA_WIDTH_SHIFT;
 +		break;
 +	case 10:
 +		dw = 2 << CSI_SENS_CONF_DATA_WIDTH_SHIFT;
 +		break;
 +	default:
 +		/*
 +		 * Actually it can only be 15 now, default is just to silence
 +		 * compiler warnings
 +		 */
 +	case 15:
 +		dw = 3 << CSI_SENS_CONF_DATA_WIDTH_SHIFT;
 +	}
 +
 +	csi_reg_write(mx3_cam, sens_conf | dw, CSI_SENS_CONF);
 +
 +	dev_dbg(dev, "Set SENS_CONF to %x\n", sens_conf | dw);
 +
 +	return 0;
 +}
 +
 +static struct soc_camera_host_ops mx3_soc_camera_host_ops = {
 +	.owner		= THIS_MODULE,
 +	.add		= mx3_camera_add_device,
 +	.remove		= mx3_camera_remove_device,
 +	.set_crop	= mx3_camera_set_crop,
 +	.set_fmt	= mx3_camera_set_fmt,
 +	.try_fmt	= mx3_camera_try_fmt,
 +	.get_formats	= mx3_camera_get_formats,
 +	.init_videobuf2	= mx3_camera_init_videobuf,
 +	.reqbufs	= mx3_camera_reqbufs,
 +	.poll		= mx3_camera_poll,
 +	.querycap	= mx3_camera_querycap,
 +	.set_bus_param	= mx3_camera_set_bus_param,
 +};
 +
 +static int __devinit mx3_camera_probe(struct platform_device *pdev)
 +{
 +	struct mx3_camera_dev *mx3_cam;
 +	struct resource *res;
 +	void __iomem *base;
 +	int err = 0;
 +	struct soc_camera_host *soc_host;
 +
 +	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 +	if (!res) {
 +		err = -ENODEV;
 +		goto egetres;
 +	}
 +
 +	mx3_cam = vzalloc(sizeof(*mx3_cam));
 +	if (!mx3_cam) {
 +		dev_err(&pdev->dev, "Could not allocate mx3 camera object\n");
 +		err = -ENOMEM;
 +		goto ealloc;
 +	}
 +
 +	mx3_cam->clk = clk_get(&pdev->dev, NULL);
 +	if (IS_ERR(mx3_cam->clk)) {
 +		err = PTR_ERR(mx3_cam->clk);
 +		goto eclkget;
 +	}
 +
 +	mx3_cam->pdata = pdev->dev.platform_data;
 +	mx3_cam->platform_flags = mx3_cam->pdata->flags;
 +	if (!(mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_MASK)) {
 +		/*
 +		 * Platform hasn't set available data widths. This is bad.
 +		 * Warn and use a default.
 +		 */
 +		dev_warn(&pdev->dev, "WARNING! Platform hasn't set available "
 +			 "data widths, using default 8 bit\n");
 +		mx3_cam->platform_flags |= MX3_CAMERA_DATAWIDTH_8;
 +	}
 +	if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_4)
 +		mx3_cam->width_flags = 1 << 3;
 +	if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_8)
 +		mx3_cam->width_flags |= 1 << 7;
 +	if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_10)
 +		mx3_cam->width_flags |= 1 << 9;
 +	if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_15)
 +		mx3_cam->width_flags |= 1 << 14;
 +
 +	mx3_cam->mclk = mx3_cam->pdata->mclk_10khz * 10000;
 +	if (!mx3_cam->mclk) {
 +		dev_warn(&pdev->dev,
 +			 "mclk_10khz == 0! Please, fix your platform data. "
 +			 "Using default 20MHz\n");
 +		mx3_cam->mclk = 20000000;
 +	}
 +
 +	/* list of video-buffers */
 +	INIT_LIST_HEAD(&mx3_cam->capture);
 +	spin_lock_init(&mx3_cam->lock);
 +
 +	base = ioremap(res->start, resource_size(res));
 +	if (!base) {
 +		pr_err("Couldn't map %x@%x\n", resource_size(res), res->start);
 +		err = -ENOMEM;
 +		goto eioremap;
 +	}
 +
 +	mx3_cam->base	= base;
 +
 +	soc_host		= &mx3_cam->soc_host;
 +	soc_host->drv_name	= MX3_CAM_DRV_NAME;
 +	soc_host->ops		= &mx3_soc_camera_host_ops;
 +	soc_host->priv		= mx3_cam;
 +	soc_host->v4l2_dev.dev	= &pdev->dev;
 +	soc_host->nr		= pdev->id;
 +
 +	mx3_cam->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
 +	if (IS_ERR(mx3_cam->alloc_ctx)) {
 +		err = PTR_ERR(mx3_cam->alloc_ctx);
 +		goto eallocctx;
 +	}
 +
 +	err = soc_camera_host_register(soc_host);
 +	if (err)
 +		goto ecamhostreg;
 +
 +	/* IDMAC interface */
 +	dmaengine_get();
 +
 +	return 0;
 +
 +ecamhostreg:
 +	vb2_dma_contig_cleanup_ctx(mx3_cam->alloc_ctx);
 +eallocctx:
 +	iounmap(base);
 +eioremap:
 +	clk_put(mx3_cam->clk);
 +eclkget:
 +	vfree(mx3_cam);
 +ealloc:
 +egetres:
 +	return err;
 +}
 +
 +static int __devexit mx3_camera_remove(struct platform_device *pdev)
 +{
 +	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
 +	struct mx3_camera_dev *mx3_cam = container_of(soc_host,
 +					struct mx3_camera_dev, soc_host);
 +
 +	clk_put(mx3_cam->clk);
 +
 +	soc_camera_host_unregister(soc_host);
 +
 +	iounmap(mx3_cam->base);
 +
 +	/*
 +	 * The channel has either not been allocated,
 +	 * or should have been released
 +	 */
 +	if (WARN_ON(mx3_cam->idmac_channel[0]))
 +		dma_release_channel(&mx3_cam->idmac_channel[0]->dma_chan);
 +
 +	vb2_dma_contig_cleanup_ctx(mx3_cam->alloc_ctx);
 +
 +	vfree(mx3_cam);
 +
 +	dmaengine_put();
 +
 +	return 0;
 +}
 +
 +static struct platform_driver mx3_camera_driver = {
 +	.driver 	= {
 +		.name	= MX3_CAM_DRV_NAME,
 +	},
 +	.probe		= mx3_camera_probe,
 +	.remove		= __devexit_p(mx3_camera_remove),
 +};
 +
 +module_platform_driver(mx3_camera_driver);
 +
 +MODULE_DESCRIPTION("i.MX3x SoC Camera Host driver");
 +MODULE_AUTHOR("Guennadi Liakhovetski <lg@denx.de>");
 +MODULE_LICENSE("GPL v2");
 +MODULE_VERSION("0.2.3");
 +MODULE_ALIAS("platform:" MX3_CAM_DRV_NAME);
diff --cc drivers/media/platform/soc_camera/pxa_camera.c
index 9c21e01f2c24,000000000000..1e3776d08dac
mode 100644,000000..100644
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ b/drivers/media/platform/soc_camera/pxa_camera.c
@@@ -1,1852 -1,0 +1,1852 @@@
 +/*
 + * V4L2 Driver for PXA camera host
 + *
 + * Copyright (C) 2006, Sascha Hauer, Pengutronix
 + * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
 + *
 + * This program is free software; you can redistribute it and/or modify
 + * it under the terms of the GNU General Public License as published by
 + * the Free Software Foundation; either version 2 of the License, or
 + * (at your option) any later version.
 + */
 +
 +#include <linux/init.h>
 +#include <linux/module.h>
 +#include <linux/io.h>
 +#include <linux/delay.h>
 +#include <linux/dma-mapping.h>
 +#include <linux/errno.h>
 +#include <linux/fs.h>
 +#include <linux/interrupt.h>
 +#include <linux/kernel.h>
 +#include <linux/mm.h>
 +#include <linux/moduleparam.h>
 +#include <linux/time.h>
 +#include <linux/device.h>
 +#include <linux/platform_device.h>
 +#include <linux/clk.h>
 +#include <linux/sched.h>
 +#include <linux/slab.h>
 +
 +#include <media/v4l2-common.h>
 +#include <media/v4l2-dev.h>
 +#include <media/videobuf-dma-sg.h>
 +#include <media/soc_camera.h>
 +#include <media/soc_mediabus.h>
 +
 +#include <linux/videodev2.h>
 +
 +#include <mach/dma.h>
- #include <mach/camera.h>
++#include <linux/platform_data/camera-pxa.h>
 +
 +#define PXA_CAM_VERSION "0.0.6"
 +#define PXA_CAM_DRV_NAME "pxa27x-camera"
 +
 +/* Camera Interface */
 +#define CICR0		0x0000
 +#define CICR1		0x0004
 +#define CICR2		0x0008
 +#define CICR3		0x000C
 +#define CICR4		0x0010
 +#define CISR		0x0014
 +#define CIFR		0x0018
 +#define CITOR		0x001C
 +#define CIBR0		0x0028
 +#define CIBR1		0x0030
 +#define CIBR2		0x0038
 +
 +#define CICR0_DMAEN	(1 << 31)	/* DMA request enable */
 +#define CICR0_PAR_EN	(1 << 30)	/* Parity enable */
 +#define CICR0_SL_CAP_EN	(1 << 29)	/* Capture enable for slave mode */
 +#define CICR0_ENB	(1 << 28)	/* Camera interface enable */
 +#define CICR0_DIS	(1 << 27)	/* Camera interface disable */
 +#define CICR0_SIM	(0x7 << 24)	/* Sensor interface mode mask */
 +#define CICR0_TOM	(1 << 9)	/* Time-out mask */
 +#define CICR0_RDAVM	(1 << 8)	/* Receive-data-available mask */
 +#define CICR0_FEM	(1 << 7)	/* FIFO-empty mask */
 +#define CICR0_EOLM	(1 << 6)	/* End-of-line mask */
 +#define CICR0_PERRM	(1 << 5)	/* Parity-error mask */
 +#define CICR0_QDM	(1 << 4)	/* Quick-disable mask */
 +#define CICR0_CDM	(1 << 3)	/* Disable-done mask */
 +#define CICR0_SOFM	(1 << 2)	/* Start-of-frame mask */
 +#define CICR0_EOFM	(1 << 1)	/* End-of-frame mask */
 +#define CICR0_FOM	(1 << 0)	/* FIFO-overrun mask */
 +
 +#define CICR1_TBIT	(1 << 31)	/* Transparency bit */
 +#define CICR1_RGBT_CONV	(0x3 << 29)	/* RGBT conversion mask */
 +#define CICR1_PPL	(0x7ff << 15)	/* Pixels per line mask */
 +#define CICR1_RGB_CONV	(0x7 << 12)	/* RGB conversion mask */
 +#define CICR1_RGB_F	(1 << 11)	/* RGB format */
 +#define CICR1_YCBCR_F	(1 << 10)	/* YCbCr format */
 +#define CICR1_RGB_BPP	(0x7 << 7)	/* RGB bis per pixel mask */
 +#define CICR1_RAW_BPP	(0x3 << 5)	/* Raw bis per pixel mask */
 +#define CICR1_COLOR_SP	(0x3 << 3)	/* Color space mask */
 +#define CICR1_DW	(0x7 << 0)	/* Data width mask */
 +
 +#define CICR2_BLW	(0xff << 24)	/* Beginning-of-line pixel clock
 +					   wait count mask */
 +#define CICR2_ELW	(0xff << 16)	/* End-of-line pixel clock
 +					   wait count mask */
 +#define CICR2_HSW	(0x3f << 10)	/* Horizontal sync pulse width mask */
 +#define CICR2_BFPW	(0x3f << 3)	/* Beginning-of-frame pixel clock
 +					   wait count mask */
 +#define CICR2_FSW	(0x7 << 0)	/* Frame stabilization
 +					   wait count mask */
 +
 +#define CICR3_BFW	(0xff << 24)	/* Beginning-of-frame line clock
 +					   wait count mask */
 +#define CICR3_EFW	(0xff << 16)	/* End-of-frame line clock
 +					   wait count mask */
 +#define CICR3_VSW	(0x3f << 10)	/* Vertical sync pulse width mask */
 +#define CICR3_BFPW	(0x3f << 3)	/* Beginning-of-frame pixel clock
 +					   wait count mask */
 +#define CICR3_LPF	(0x7ff << 0)	/* Lines per frame mask */
 +
 +#define CICR4_MCLK_DLY	(0x3 << 24)	/* MCLK Data Capture Delay mask */
 +#define CICR4_PCLK_EN	(1 << 23)	/* Pixel clock enable */
 +#define CICR4_PCP	(1 << 22)	/* Pixel clock polarity */
 +#define CICR4_HSP	(1 << 21)	/* Horizontal sync polarity */
 +#define CICR4_VSP	(1 << 20)	/* Vertical sync polarity */
 +#define CICR4_MCLK_EN	(1 << 19)	/* MCLK enable */
 +#define CICR4_FR_RATE	(0x7 << 8)	/* Frame rate mask */
 +#define CICR4_DIV	(0xff << 0)	/* Clock divisor mask */
 +
 +#define CISR_FTO	(1 << 15)	/* FIFO time-out */
 +#define CISR_RDAV_2	(1 << 14)	/* Channel 2 receive data available */
 +#define CISR_RDAV_1	(1 << 13)	/* Channel 1 receive data available */
 +#define CISR_RDAV_0	(1 << 12)	/* Channel 0 receive data available */
 +#define CISR_FEMPTY_2	(1 << 11)	/* Channel 2 FIFO empty */
 +#define CISR_FEMPTY_1	(1 << 10)	/* Channel 1 FIFO empty */
 +#define CISR_FEMPTY_0	(1 << 9)	/* Channel 0 FIFO empty */
 +#define CISR_EOL	(1 << 8)	/* End of line */
 +#define CISR_PAR_ERR	(1 << 7)	/* Parity error */
 +#define CISR_CQD	(1 << 6)	/* Camera interface quick disable */
 +#define CISR_CDD	(1 << 5)	/* Camera interface disable done */
 +#define CISR_SOF	(1 << 4)	/* Start of frame */
 +#define CISR_EOF	(1 << 3)	/* End of frame */
 +#define CISR_IFO_2	(1 << 2)	/* FIFO overrun for Channel 2 */
 +#define CISR_IFO_1	(1 << 1)	/* FIFO overrun for Channel 1 */
 +#define CISR_IFO_0	(1 << 0)	/* FIFO overrun for Channel 0 */
 +
 +#define CIFR_FLVL2	(0x7f << 23)	/* FIFO 2 level mask */
 +#define CIFR_FLVL1	(0x7f << 16)	/* FIFO 1 level mask */
 +#define CIFR_FLVL0	(0xff << 8)	/* FIFO 0 level mask */
 +#define CIFR_THL_0	(0x3 << 4)	/* Threshold Level for Channel 0 FIFO */
 +#define CIFR_RESET_F	(1 << 3)	/* Reset input FIFOs */
 +#define CIFR_FEN2	(1 << 2)	/* FIFO enable for channel 2 */
 +#define CIFR_FEN1	(1 << 1)	/* FIFO enable for channel 1 */
 +#define CIFR_FEN0	(1 << 0)	/* FIFO enable for channel 0 */
 +
 +#define CICR0_SIM_MP	(0 << 24)
 +#define CICR0_SIM_SP	(1 << 24)
 +#define CICR0_SIM_MS	(2 << 24)
 +#define CICR0_SIM_EP	(3 << 24)
 +#define CICR0_SIM_ES	(4 << 24)
 +
 +#define CICR1_DW_VAL(x)   ((x) & CICR1_DW)	    /* Data bus width */
 +#define CICR1_PPL_VAL(x)  (((x) << 15) & CICR1_PPL) /* Pixels per line */
 +#define CICR1_COLOR_SP_VAL(x)	(((x) << 3) & CICR1_COLOR_SP)	/* color space */
 +#define CICR1_RGB_BPP_VAL(x)	(((x) << 7) & CICR1_RGB_BPP)	/* bpp for rgb */
 +#define CICR1_RGBT_CONV_VAL(x)	(((x) << 29) & CICR1_RGBT_CONV)	/* rgbt conv */
 +
 +#define CICR2_BLW_VAL(x)  (((x) << 24) & CICR2_BLW) /* Beginning-of-line pixel clock wait count */
 +#define CICR2_ELW_VAL(x)  (((x) << 16) & CICR2_ELW) /* End-of-line pixel clock wait count */
 +#define CICR2_HSW_VAL(x)  (((x) << 10) & CICR2_HSW) /* Horizontal sync pulse width */
 +#define CICR2_BFPW_VAL(x) (((x) << 3) & CICR2_BFPW) /* Beginning-of-frame pixel clock wait count */
 +#define CICR2_FSW_VAL(x)  (((x) << 0) & CICR2_FSW)  /* Frame stabilization wait count */
 +
 +#define CICR3_BFW_VAL(x)  (((x) << 24) & CICR3_BFW) /* Beginning-of-frame line clock wait count  */
 +#define CICR3_EFW_VAL(x)  (((x) << 16) & CICR3_EFW) /* End-of-frame line clock wait count */
 +#define CICR3_VSW_VAL(x)  (((x) << 11) & CICR3_VSW) /* Vertical sync pulse width */
 +#define CICR3_LPF_VAL(x)  (((x) << 0) & CICR3_LPF)  /* Lines per frame */
 +
 +#define CICR0_IRQ_MASK (CICR0_TOM | CICR0_RDAVM | CICR0_FEM | CICR0_EOLM | \
 +			CICR0_PERRM | CICR0_QDM | CICR0_CDM | CICR0_SOFM | \
 +			CICR0_EOFM | CICR0_FOM)
 +
 +/*
 + * Structures
 + */
 +enum pxa_camera_active_dma {
 +	DMA_Y = 0x1,
 +	DMA_U = 0x2,
 +	DMA_V = 0x4,
 +};
 +
 +/* descriptor needed for the PXA DMA engine */
 +struct pxa_cam_dma {
 +	dma_addr_t		sg_dma;
 +	struct pxa_dma_desc	*sg_cpu;
 +	size_t			sg_size;
 +	int			sglen;
 +};
 +
 +/* buffer for one video frame */
 +struct pxa_buffer {
 +	/* common v4l buffer stuff -- must be first */
 +	struct videobuf_buffer		vb;
 +	enum v4l2_mbus_pixelcode	code;
 +	/* our descriptor lists for Y, U and V channels */
 +	struct pxa_cam_dma		dmas[3];
 +	int				inwork;
 +	enum pxa_camera_active_dma	active_dma;
 +};
 +
 +struct pxa_camera_dev {
 +	struct soc_camera_host	soc_host;
 +	/*
 +	 * PXA27x is only supposed to handle one camera on its Quick Capture
 +	 * interface. If anyone ever builds hardware to enable more than
 +	 * one camera, they will have to modify this driver too
 +	 */
 +	struct soc_camera_device *icd;
 +	struct clk		*clk;
 +
 +	unsigned int		irq;
 +	void __iomem		*base;
 +
 +	int			channels;
 +	unsigned int		dma_chans[3];
 +
 +	struct pxacamera_platform_data *pdata;
 +	struct resource		*res;
 +	unsigned long		platform_flags;
 +	unsigned long		ciclk;
 +	unsigned long		mclk;
 +	u32			mclk_divisor;
 +	u16			width_flags;	/* max 10 bits */
 +
 +	struct list_head	capture;
 +
 +	spinlock_t		lock;
 +
 +	struct pxa_buffer	*active;
 +	struct pxa_dma_desc	*sg_tail[3];
 +
 +	u32			save_cicr[5];
 +};
 +
 +struct pxa_cam {
 +	unsigned long flags;
 +};
 +
 +static const char *pxa_cam_driver_description = "PXA_Camera";
 +
 +static unsigned int vid_limit = 16;	/* Video memory limit, in Mb */
 +
 +/*
 + *  Videobuf operations
 + */
 +static int pxa_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 +			      unsigned int *size)
 +{
 +	struct soc_camera_device *icd = vq->priv_data;
 +
 +	dev_dbg(icd->parent, "count=%d, size=%d\n", *count, *size);
 +
 +	*size = icd->sizeimage;
 +
 +	if (0 == *count)
 +		*count = 32;
 +	if (*size * *count > vid_limit * 1024 * 1024)
 +		*count = (vid_limit * 1024 * 1024) / *size;
 +
 +	return 0;
 +}
 +
 +static void free_buffer(struct videobuf_queue *vq, struct pxa_buffer *buf)
 +{
 +	struct soc_camera_device *icd = vq->priv_data;
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
 +	int i;
 +
 +	BUG_ON(in_interrupt());
 +
 +	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 +		&buf->vb, buf->vb.baddr, buf->vb.bsize);
 +
 +	/*
 +	 * This waits until this buffer is out of danger, i.e., until it is no
 +	 * longer in STATE_QUEUED or STATE_ACTIVE
 +	 */
 +	videobuf_waiton(vq, &buf->vb, 0, 0);
 +	videobuf_dma_unmap(vq->dev, dma);
 +	videobuf_dma_free(dma);
 +
 +	for (i = 0; i < ARRAY_SIZE(buf->dmas); i++) {
 +		if (buf->dmas[i].sg_cpu)
 +			dma_free_coherent(ici->v4l2_dev.dev,
 +					  buf->dmas[i].sg_size,
 +					  buf->dmas[i].sg_cpu,
 +					  buf->dmas[i].sg_dma);
 +		buf->dmas[i].sg_cpu = NULL;
 +	}
 +
 +	buf->vb.state = VIDEOBUF_NEEDS_INIT;
 +}
 +
 +static int calculate_dma_sglen(struct scatterlist *sglist, int sglen,
 +			       int sg_first_ofs, int size)
 +{
 +	int i, offset, dma_len, xfer_len;
 +	struct scatterlist *sg;
 +
 +	offset = sg_first_ofs;
 +	for_each_sg(sglist, sg, sglen, i) {
 +		dma_len = sg_dma_len(sg);
 +
 +		/* PXA27x Developer's Manual 27.4.4.1: round up to 8 bytes */
 +		xfer_len = roundup(min(dma_len - offset, size), 8);
 +
 +		size = max(0, size - xfer_len);
 +		offset = 0;
 +		if (size == 0)
 +			break;
 +	}
 +
 +	BUG_ON(size != 0);
 +	return i + 1;
 +}
 +
 +/**
 + * pxa_init_dma_channel - init dma descriptors
 + * @pcdev: pxa camera device
 + * @buf: pxa buffer to find pxa dma channel
 + * @dma: dma video buffer
 + * @channel: dma channel (0 => 'Y', 1 => 'U', 2 => 'V')
 + * @cibr: camera Receive Buffer Register
 + * @size: bytes to transfer
 + * @sg_first: first element of sg_list
 + * @sg_first_ofs: offset in first element of sg_list
 + *
 + * Prepares the pxa dma descriptors to transfer one camera channel.
 + * Beware sg_first and sg_first_ofs are both input and output parameters.
 + *
 + * Returns 0 or -ENOMEM if no coherent memory is available
 + */
 +static int pxa_init_dma_channel(struct pxa_camera_dev *pcdev,
 +				struct pxa_buffer *buf,
 +				struct videobuf_dmabuf *dma, int channel,
 +				int cibr, int size,
 +				struct scatterlist **sg_first, int *sg_first_ofs)
 +{
 +	struct pxa_cam_dma *pxa_dma = &buf->dmas[channel];
 +	struct device *dev = pcdev->soc_host.v4l2_dev.dev;
 +	struct scatterlist *sg;
 +	int i, offset, sglen;
 +	int dma_len = 0, xfer_len = 0;
 +
 +	if (pxa_dma->sg_cpu)
 +		dma_free_coherent(dev, pxa_dma->sg_size,
 +				  pxa_dma->sg_cpu, pxa_dma->sg_dma);
 +
 +	sglen = calculate_dma_sglen(*sg_first, dma->sglen,
 +				    *sg_first_ofs, size);
 +
 +	pxa_dma->sg_size = (sglen + 1) * sizeof(struct pxa_dma_desc);
 +	pxa_dma->sg_cpu = dma_alloc_coherent(dev, pxa_dma->sg_size,
 +					     &pxa_dma->sg_dma, GFP_KERNEL);
 +	if (!pxa_dma->sg_cpu)
 +		return -ENOMEM;
 +
 +	pxa_dma->sglen = sglen;
 +	offset = *sg_first_ofs;
 +
 +	dev_dbg(dev, "DMA: sg_first=%p, sglen=%d, ofs=%d, dma.desc=%x\n",
 +		*sg_first, sglen, *sg_first_ofs, pxa_dma->sg_dma);
 +
 +
 +	for_each_sg(*sg_first, sg, sglen, i) {
 +		dma_len = sg_dma_len(sg);
 +
 +		/* PXA27x Developer's Manual 27.4.4.1: round up to 8 bytes */
 +		xfer_len = roundup(min(dma_len - offset, size), 8);
 +
 +		size = max(0, size - xfer_len);
 +
 +		pxa_dma->sg_cpu[i].dsadr = pcdev->res->start + cibr;
 +		pxa_dma->sg_cpu[i].dtadr = sg_dma_address(sg) + offset;
 +		pxa_dma->sg_cpu[i].dcmd =
 +			DCMD_FLOWSRC | DCMD_BURST8 | DCMD_INCTRGADDR | xfer_len;
 +#ifdef DEBUG
 +		if (!i)
 +			pxa_dma->sg_cpu[i].dcmd |= DCMD_STARTIRQEN;
 +#endif
 +		pxa_dma->sg_cpu[i].ddadr =
 +			pxa_dma->sg_dma + (i + 1) * sizeof(struct pxa_dma_desc);
 +
 +		dev_vdbg(dev, "DMA: desc.%08x->@phys=0x%08x, len=%d\n",
 +			 pxa_dma->sg_dma + i * sizeof(struct pxa_dma_desc),
 +			 sg_dma_address(sg) + offset, xfer_len);
 +		offset = 0;
 +
 +		if (size == 0)
 +			break;
 +	}
 +
 +	pxa_dma->sg_cpu[sglen].ddadr = DDADR_STOP;
 +	pxa_dma->sg_cpu[sglen].dcmd  = DCMD_FLOWSRC | DCMD_BURST8 | DCMD_ENDIRQEN;
 +
 +	/*
 +	 * Handle 1 special case :
 +	 *  - in 3 planes (YUV422P format), we might finish with xfer_len equal
 +	 *    to dma_len (end on PAGE boundary). In this case, the sg element
 +	 *    for next plane should be the next after the last used to store the
 +	 *    last scatter gather RAM page
 +	 */
 +	if (xfer_len >= dma_len) {
 +		*sg_first_ofs = xfer_len - dma_len;
 +		*sg_first = sg_next(sg);
 +	} else {
 +		*sg_first_ofs = xfer_len;
 +		*sg_first = sg;
 +	}
 +
 +	return 0;
 +}
 +
 +static void pxa_videobuf_set_actdma(struct pxa_camera_dev *pcdev,
 +				    struct pxa_buffer *buf)
 +{
 +	buf->active_dma = DMA_Y;
 +	if (pcdev->channels == 3)
 +		buf->active_dma |= DMA_U | DMA_V;
 +}
 +
 +/*
 + * Please check the DMA prepared buffer structure in :
 + *   Documentation/video4linux/pxa_camera.txt
 + * Please check also in pxa_camera_check_link_miss() to understand why DMA chain
 + * modification while DMA chain is running will work anyway.
 + */
 +static int pxa_videobuf_prepare(struct videobuf_queue *vq,
 +		struct videobuf_buffer *vb, enum v4l2_field field)
 +{
 +	struct soc_camera_device *icd = vq->priv_data;
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct pxa_camera_dev *pcdev = ici->priv;
 +	struct device *dev = pcdev->soc_host.v4l2_dev.dev;
 +	struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
 +	int ret;
 +	int size_y, size_u = 0, size_v = 0;
 +
 +	dev_dbg(dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 +		vb, vb->baddr, vb->bsize);
 +
 +	/* Added list head initialization on alloc */
 +	WARN_ON(!list_empty(&vb->queue));
 +
 +#ifdef DEBUG
 +	/*
 +	 * This can be useful if you want to see if we actually fill
 +	 * the buffer with something
 +	 */
 +	memset((void *)vb->baddr, 0xaa, vb->bsize);
 +#endif
 +
 +	BUG_ON(NULL == icd->current_fmt);
 +
 +	/*
 +	 * I think, in buf_prepare you only have to protect global data,
 +	 * the actual buffer is yours
 +	 */
 +	buf->inwork = 1;
 +
 +	if (buf->code	!= icd->current_fmt->code ||
 +	    vb->width	!= icd->user_width ||
 +	    vb->height	!= icd->user_height ||
 +	    vb->field	!= field) {
 +		buf->code	= icd->current_fmt->code;
 +		vb->width	= icd->user_width;
 +		vb->height	= icd->user_height;
 +		vb->field	= field;
 +		vb->state	= VIDEOBUF_NEEDS_INIT;
 +	}
 +
 +	vb->size = icd->sizeimage;
 +	if (0 != vb->baddr && vb->bsize < vb->size) {
 +		ret = -EINVAL;
 +		goto out;
 +	}
 +
 +	if (vb->state == VIDEOBUF_NEEDS_INIT) {
 +		int size = vb->size;
 +		int next_ofs = 0;
 +		struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
 +		struct scatterlist *sg;
 +
 +		ret = videobuf_iolock(vq, vb, NULL);
 +		if (ret)
 +			goto fail;
 +
 +		if (pcdev->channels == 3) {
 +			size_y = size / 2;
 +			size_u = size_v = size / 4;
 +		} else {
 +			size_y = size;
 +		}
 +
 +		sg = dma->sglist;
 +
 +		/* init DMA for Y channel */
 +		ret = pxa_init_dma_channel(pcdev, buf, dma, 0, CIBR0, size_y,
 +					   &sg, &next_ofs);
 +		if (ret) {
 +			dev_err(dev, "DMA initialization for Y/RGB failed\n");
 +			goto fail;
 +		}
 +
 +		/* init DMA for U channel */
 +		if (size_u)
 +			ret = pxa_init_dma_channel(pcdev, buf, dma, 1, CIBR1,
 +						   size_u, &sg, &next_ofs);
 +		if (ret) {
 +			dev_err(dev, "DMA initialization for U failed\n");
 +			goto fail_u;
 +		}
 +
 +		/* init DMA for V channel */
 +		if (size_v)
 +			ret = pxa_init_dma_channel(pcdev, buf, dma, 2, CIBR2,
 +						   size_v, &sg, &next_ofs);
 +		if (ret) {
 +			dev_err(dev, "DMA initialization for V failed\n");
 +			goto fail_v;
 +		}
 +
 +		vb->state = VIDEOBUF_PREPARED;
 +	}
 +
 +	buf->inwork = 0;
 +	pxa_videobuf_set_actdma(pcdev, buf);
 +
 +	return 0;
 +
 +fail_v:
 +	dma_free_coherent(dev, buf->dmas[1].sg_size,
 +			  buf->dmas[1].sg_cpu, buf->dmas[1].sg_dma);
 +fail_u:
 +	dma_free_coherent(dev, buf->dmas[0].sg_size,
 +			  buf->dmas[0].sg_cpu, buf->dmas[0].sg_dma);
 +fail:
 +	free_buffer(vq, buf);
 +out:
 +	buf->inwork = 0;
 +	return ret;
 +}
 +
 +/**
 + * pxa_dma_start_channels - start DMA channel for active buffer
 + * @pcdev: pxa camera device
 + *
 + * Initialize DMA channels to the beginning of the active video buffer, and
 + * start these channels.
 + */
 +static void pxa_dma_start_channels(struct pxa_camera_dev *pcdev)
 +{
 +	int i;
 +	struct pxa_buffer *active;
 +
 +	active = pcdev->active;
 +
 +	for (i = 0; i < pcdev->channels; i++) {
 +		dev_dbg(pcdev->soc_host.v4l2_dev.dev,
 +			"%s (channel=%d) ddadr=%08x\n", __func__,
 +			i, active->dmas[i].sg_dma);
 +		DDADR(pcdev->dma_chans[i]) = active->dmas[i].sg_dma;
 +		DCSR(pcdev->dma_chans[i]) = DCSR_RUN;
 +	}
 +}
 +
 +static void pxa_dma_stop_channels(struct pxa_camera_dev *pcdev)
 +{
 +	int i;
 +
 +	for (i = 0; i < pcdev->channels; i++) {
 +		dev_dbg(pcdev->soc_host.v4l2_dev.dev,
 +			"%s (channel=%d)\n", __func__, i);
 +		DCSR(pcdev->dma_chans[i]) = 0;
 +	}
 +}
 +
 +static void pxa_dma_add_tail_buf(struct pxa_camera_dev *pcdev,
 +				 struct pxa_buffer *buf)
 +{
 +	int i;
 +	struct pxa_dma_desc *buf_last_desc;
 +
 +	for (i = 0; i < pcdev->channels; i++) {
 +		buf_last_desc = buf->dmas[i].sg_cpu + buf->dmas[i].sglen;
 +		buf_last_desc->ddadr = DDADR_STOP;
 +
 +		if (pcdev->sg_tail[i])
 +			/* Link the new buffer to the old tail */
 +			pcdev->sg_tail[i]->ddadr = buf->dmas[i].sg_dma;
 +
 +		/* Update the channel tail */
 +		pcdev->sg_tail[i] = buf_last_desc;
 +	}
 +}
 +
 +/**
 + * pxa_camera_start_capture - start video capturing
 + * @pcdev: camera device
 + *
 + * Launch capturing. DMA channels should not be active yet. They should get
 + * activated at the end of frame interrupt, to capture only whole frames, and
 + * never begin the capture of a partial frame.
 + */
 +static void pxa_camera_start_capture(struct pxa_camera_dev *pcdev)
 +{
 +	unsigned long cicr0;
 +
 +	dev_dbg(pcdev->soc_host.v4l2_dev.dev, "%s\n", __func__);
 +	/* Enable End-Of-Frame Interrupt */
 +	cicr0 = __raw_readl(pcdev->base + CICR0) | CICR0_ENB;
 +	cicr0 &= ~CICR0_EOFM;
 +	__raw_writel(cicr0, pcdev->base + CICR0);
 +}
 +
 +static void pxa_camera_stop_capture(struct pxa_camera_dev *pcdev)
 +{
 +	unsigned long cicr0;
 +
 +	pxa_dma_stop_channels(pcdev);
 +
 +	cicr0 = __raw_readl(pcdev->base + CICR0) & ~CICR0_ENB;
 +	__raw_writel(cicr0, pcdev->base + CICR0);
 +
 +	pcdev->active = NULL;
 +	dev_dbg(pcdev->soc_host.v4l2_dev.dev, "%s\n", __func__);
 +}
 +
 +/* Called under spinlock_irqsave(&pcdev->lock, ...) */
 +static void pxa_videobuf_queue(struct videobuf_queue *vq,
 +			       struct videobuf_buffer *vb)
 +{
 +	struct soc_camera_device *icd = vq->priv_data;
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct pxa_camera_dev *pcdev = ici->priv;
 +	struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
 +
 +	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d active=%p\n",
 +		__func__, vb, vb->baddr, vb->bsize, pcdev->active);
 +
 +	list_add_tail(&vb->queue, &pcdev->capture);
 +
 +	vb->state = VIDEOBUF_ACTIVE;
 +	pxa_dma_add_tail_buf(pcdev, buf);
 +
 +	if (!pcdev->active)
 +		pxa_camera_start_capture(pcdev);
 +}
 +
 +static void pxa_videobuf_release(struct videobuf_queue *vq,
 +				 struct videobuf_buffer *vb)
 +{
 +	struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
 +#ifdef DEBUG
 +	struct soc_camera_device *icd = vq->priv_data;
 +	struct device *dev = icd->parent;
 +
 +	dev_dbg(dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 +		vb, vb->baddr, vb->bsize);
 +
 +	switch (vb->state) {
 +	case VIDEOBUF_ACTIVE:
 +		dev_dbg(dev, "%s (active)\n", __func__);
 +		break;
 +	case VIDEOBUF_QUEUED:
 +		dev_dbg(dev, "%s (queued)\n", __func__);
 +		break;
 +	case VIDEOBUF_PREPARED:
 +		dev_dbg(dev, "%s (prepared)\n", __func__);
 +		break;
 +	default:
 +		dev_dbg(dev, "%s (unknown)\n", __func__);
 +		break;
 +	}
 +#endif
 +
 +	free_buffer(vq, buf);
 +}
 +
 +static void pxa_camera_wakeup(struct pxa_camera_dev *pcdev,
 +			      struct videobuf_buffer *vb,
 +			      struct pxa_buffer *buf)
 +{
 +	int i;
 +
 +	/* _init is used to debug races, see comment in pxa_camera_reqbufs() */
 +	list_del_init(&vb->queue);
 +	vb->state = VIDEOBUF_DONE;
 +	do_gettimeofday(&vb->ts);
 +	vb->field_count++;
 +	wake_up(&vb->done);
 +	dev_dbg(pcdev->soc_host.v4l2_dev.dev, "%s dequeud buffer (vb=0x%p)\n",
 +		__func__, vb);
 +
 +	if (list_empty(&pcdev->capture)) {
 +		pxa_camera_stop_capture(pcdev);
 +		for (i = 0; i < pcdev->channels; i++)
 +			pcdev->sg_tail[i] = NULL;
 +		return;
 +	}
 +
 +	pcdev->active = list_entry(pcdev->capture.next,
 +				   struct pxa_buffer, vb.queue);
 +}
 +
 +/**
 + * pxa_camera_check_link_miss - check missed DMA linking
 + * @pcdev: camera device
 + *
 + * The DMA chaining is done with DMA running. This means a tiny temporal window
 + * remains, where a buffer is queued on the chain, while the chain is already
 + * stopped. This means the tailed buffer would never be transferred by DMA.
 + * This function restarts the capture for this corner case, where :
 + *  - DADR() == DADDR_STOP
 + *  - a videobuffer is queued on the pcdev->capture list
 + *
 + * Please check the "DMA hot chaining timeslice issue" in
 + *   Documentation/video4linux/pxa_camera.txt
 + *
 + * Context: should only be called within the dma irq handler
 + */
 +static void pxa_camera_check_link_miss(struct pxa_camera_dev *pcdev)
 +{
 +	int i, is_dma_stopped = 1;
 +
 +	for (i = 0; i < pcdev->channels; i++)
 +		if (DDADR(pcdev->dma_chans[i]) != DDADR_STOP)
 +			is_dma_stopped = 0;
 +	dev_dbg(pcdev->soc_host.v4l2_dev.dev,
 +		"%s : top queued buffer=%p, dma_stopped=%d\n",
 +		__func__, pcdev->active, is_dma_stopped);
 +	if (pcdev->active && is_dma_stopped)
 +		pxa_camera_start_capture(pcdev);
 +}
 +
 +static void pxa_camera_dma_irq(int channel, struct pxa_camera_dev *pcdev,
 +			       enum pxa_camera_active_dma act_dma)
 +{
 +	struct device *dev = pcdev->soc_host.v4l2_dev.dev;
 +	struct pxa_buffer *buf;
 +	unsigned long flags;
 +	u32 status, camera_status, overrun;
 +	struct videobuf_buffer *vb;
 +
 +	spin_lock_irqsave(&pcdev->lock, flags);
 +
 +	status = DCSR(channel);
 +	DCSR(channel) = status;
 +
 +	camera_status = __raw_readl(pcdev->base + CISR);
 +	overrun = CISR_IFO_0;
 +	if (pcdev->channels == 3)
 +		overrun |= CISR_IFO_1 | CISR_IFO_2;
 +
 +	if (status & DCSR_BUSERR) {
 +		dev_err(dev, "DMA Bus Error IRQ!\n");
 +		goto out;
 +	}
 +
 +	if (!(status & (DCSR_ENDINTR | DCSR_STARTINTR))) {
 +		dev_err(dev, "Unknown DMA IRQ source, status: 0x%08x\n",
 +			status);
 +		goto out;
 +	}
 +
 +	/*
 +	 * pcdev->active should not be NULL in DMA irq handler.
 +	 *
 +	 * But there is one corner case : if capture was stopped due to an
 +	 * overrun of channel 1, and at that same channel 2 was completed.
 +	 *
 +	 * When handling the overrun in DMA irq for channel 1, we'll stop the
 +	 * capture and restart it (and thus set pcdev->active to NULL). But the
 +	 * DMA irq handler will already be pending for channel 2. So on entering
 +	 * the DMA irq handler for channel 2 there will be no active buffer, yet
 +	 * that is normal.
 +	 */
 +	if (!pcdev->active)
 +		goto out;
 +
 +	vb = &pcdev->active->vb;
 +	buf = container_of(vb, struct pxa_buffer, vb);
 +	WARN_ON(buf->inwork || list_empty(&vb->queue));
 +
 +	dev_dbg(dev, "%s channel=%d %s%s(vb=0x%p) dma.desc=%x\n",
 +		__func__, channel, status & DCSR_STARTINTR ? "SOF " : "",
 +		status & DCSR_ENDINTR ? "EOF " : "", vb, DDADR(channel));
 +
 +	if (status & DCSR_ENDINTR) {
 +		/*
 +		 * It's normal if the last frame creates an overrun, as there
 +		 * are no more DMA descriptors to fetch from QCI fifos
 +		 */
 +		if (camera_status & overrun &&
 +		    !list_is_last(pcdev->capture.next, &pcdev->capture)) {
 +			dev_dbg(dev, "FIFO overrun! CISR: %x\n",
 +				camera_status);
 +			pxa_camera_stop_capture(pcdev);
 +			pxa_camera_start_capture(pcdev);
 +			goto out;
 +		}
 +		buf->active_dma &= ~act_dma;
 +		if (!buf->active_dma) {
 +			pxa_camera_wakeup(pcdev, vb, buf);
 +			pxa_camera_check_link_miss(pcdev);
 +		}
 +	}
 +
 +out:
 +	spin_unlock_irqrestore(&pcdev->lock, flags);
 +}
 +
 +static void pxa_camera_dma_irq_y(int channel, void *data)
 +{
 +	struct pxa_camera_dev *pcdev = data;
 +	pxa_camera_dma_irq(channel, pcdev, DMA_Y);
 +}
 +
 +static void pxa_camera_dma_irq_u(int channel, void *data)
 +{
 +	struct pxa_camera_dev *pcdev = data;
 +	pxa_camera_dma_irq(channel, pcdev, DMA_U);
 +}
 +
 +static void pxa_camera_dma_irq_v(int channel, void *data)
 +{
 +	struct pxa_camera_dev *pcdev = data;
 +	pxa_camera_dma_irq(channel, pcdev, DMA_V);
 +}
 +
 +static struct videobuf_queue_ops pxa_videobuf_ops = {
 +	.buf_setup      = pxa_videobuf_setup,
 +	.buf_prepare    = pxa_videobuf_prepare,
 +	.buf_queue      = pxa_videobuf_queue,
 +	.buf_release    = pxa_videobuf_release,
 +};
 +
 +static void pxa_camera_init_videobuf(struct videobuf_queue *q,
 +			      struct soc_camera_device *icd)
 +{
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct pxa_camera_dev *pcdev = ici->priv;
 +
 +	/*
 +	 * We must pass NULL as dev pointer, then all pci_* dma operations
 +	 * transform to normal dma_* ones.
 +	 */
 +	videobuf_queue_sg_init(q, &pxa_videobuf_ops, NULL, &pcdev->lock,
 +				V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
 +				sizeof(struct pxa_buffer), icd, &icd->video_lock);
 +}
 +
 +static u32 mclk_get_divisor(struct platform_device *pdev,
 +			    struct pxa_camera_dev *pcdev)
 +{
 +	unsigned long mclk = pcdev->mclk;
 +	struct device *dev = &pdev->dev;
 +	u32 div;
 +	unsigned long lcdclk;
 +
 +	lcdclk = clk_get_rate(pcdev->clk);
 +	pcdev->ciclk = lcdclk;
 +
 +	/* mclk <= ciclk / 4 (27.4.2) */
 +	if (mclk > lcdclk / 4) {
 +		mclk = lcdclk / 4;
 +		dev_warn(dev, "Limiting master clock to %lu\n", mclk);
 +	}
 +
 +	/* We verify mclk != 0, so if anyone breaks it, here comes their Oops */
 +	div = (lcdclk + 2 * mclk - 1) / (2 * mclk) - 1;
 +
 +	/* If we're not supplying MCLK, leave it at 0 */
 +	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
 +		pcdev->mclk = lcdclk / (2 * (div + 1));
 +
 +	dev_dbg(dev, "LCD clock %luHz, target freq %luHz, divisor %u\n",
 +		lcdclk, mclk, div);
 +
 +	return div;
 +}
 +
 +static void recalculate_fifo_timeout(struct pxa_camera_dev *pcdev,
 +				     unsigned long pclk)
 +{
 +	/* We want a timeout > 1 pixel time, not ">=" */
 +	u32 ciclk_per_pixel = pcdev->ciclk / pclk + 1;
 +
 +	__raw_writel(ciclk_per_pixel, pcdev->base + CITOR);
 +}
 +
 +static void pxa_camera_activate(struct pxa_camera_dev *pcdev)
 +{
 +	u32 cicr4 = 0;
 +
 +	/* disable all interrupts */
 +	__raw_writel(0x3ff, pcdev->base + CICR0);
 +
 +	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
 +		cicr4 |= CICR4_PCLK_EN;
 +	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
 +		cicr4 |= CICR4_MCLK_EN;
 +	if (pcdev->platform_flags & PXA_CAMERA_PCP)
 +		cicr4 |= CICR4_PCP;
 +	if (pcdev->platform_flags & PXA_CAMERA_HSP)
 +		cicr4 |= CICR4_HSP;
 +	if (pcdev->platform_flags & PXA_CAMERA_VSP)
 +		cicr4 |= CICR4_VSP;
 +
 +	__raw_writel(pcdev->mclk_divisor | cicr4, pcdev->base + CICR4);
 +
 +	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
 +		/* Initialise the timeout under the assumption pclk = mclk */
 +		recalculate_fifo_timeout(pcdev, pcdev->mclk);
 +	else
 +		/* "Safe default" - 13MHz */
 +		recalculate_fifo_timeout(pcdev, 13000000);
 +
 +	clk_prepare_enable(pcdev->clk);
 +}
 +
 +static void pxa_camera_deactivate(struct pxa_camera_dev *pcdev)
 +{
 +	clk_disable_unprepare(pcdev->clk);
 +}
 +
 +static irqreturn_t pxa_camera_irq(int irq, void *data)
 +{
 +	struct pxa_camera_dev *pcdev = data;
 +	unsigned long status, cifr, cicr0;
 +	struct pxa_buffer *buf;
 +	struct videobuf_buffer *vb;
 +
 +	status = __raw_readl(pcdev->base + CISR);
 +	dev_dbg(pcdev->soc_host.v4l2_dev.dev,
 +		"Camera interrupt status 0x%lx\n", status);
 +
 +	if (!status)
 +		return IRQ_NONE;
 +
 +	__raw_writel(status, pcdev->base + CISR);
 +
 +	if (status & CISR_EOF) {
 +		/* Reset the FIFOs */
 +		cifr = __raw_readl(pcdev->base + CIFR) | CIFR_RESET_F;
 +		__raw_writel(cifr, pcdev->base + CIFR);
 +
 +		pcdev->active = list_first_entry(&pcdev->capture,
 +					   struct pxa_buffer, vb.queue);
 +		vb = &pcdev->active->vb;
 +		buf = container_of(vb, struct pxa_buffer, vb);
 +		pxa_videobuf_set_actdma(pcdev, buf);
 +
 +		pxa_dma_start_channels(pcdev);
 +
 +		cicr0 = __raw_readl(pcdev->base + CICR0) | CICR0_EOFM;
 +		__raw_writel(cicr0, pcdev->base + CICR0);
 +	}
 +
 +	return IRQ_HANDLED;
 +}
 +
 +/*
 + * The following two functions absolutely depend on the fact, that
 + * there can be only one camera on PXA quick capture interface
 + * Called with .video_lock held
 + */
 +static int pxa_camera_add_device(struct soc_camera_device *icd)
 +{
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct pxa_camera_dev *pcdev = ici->priv;
 +
 +	if (pcdev->icd)
 +		return -EBUSY;
 +
 +	pxa_camera_activate(pcdev);
 +
 +	pcdev->icd = icd;
 +
 +	dev_info(icd->parent, "PXA Camera driver attached to camera %d\n",
 +		 icd->devnum);
 +
 +	return 0;
 +}
 +
 +/* Called with .video_lock held */
 +static void pxa_camera_remove_device(struct soc_camera_device *icd)
 +{
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct pxa_camera_dev *pcdev = ici->priv;
 +
 +	BUG_ON(icd != pcdev->icd);
 +
 +	dev_info(icd->parent, "PXA Camera driver detached from camera %d\n",
 +		 icd->devnum);
 +
 +	/* disable capture, disable interrupts */
 +	__raw_writel(0x3ff, pcdev->base + CICR0);
 +
 +	/* Stop DMA engine */
 +	DCSR(pcdev->dma_chans[0]) = 0;
 +	DCSR(pcdev->dma_chans[1]) = 0;
 +	DCSR(pcdev->dma_chans[2]) = 0;
 +
 +	pxa_camera_deactivate(pcdev);
 +
 +	pcdev->icd = NULL;
 +}
 +
 +static int test_platform_param(struct pxa_camera_dev *pcdev,
 +			       unsigned char buswidth, unsigned long *flags)
 +{
 +	/*
 +	 * Platform specified synchronization and pixel clock polarities are
 +	 * only a recommendation and are only used during probing. The PXA270
 +	 * quick capture interface supports both.
 +	 */
 +	*flags = (pcdev->platform_flags & PXA_CAMERA_MASTER ?
 +		  V4L2_MBUS_MASTER : V4L2_MBUS_SLAVE) |
 +		V4L2_MBUS_HSYNC_ACTIVE_HIGH |
 +		V4L2_MBUS_HSYNC_ACTIVE_LOW |
 +		V4L2_MBUS_VSYNC_ACTIVE_HIGH |
 +		V4L2_MBUS_VSYNC_ACTIVE_LOW |
 +		V4L2_MBUS_DATA_ACTIVE_HIGH |
 +		V4L2_MBUS_PCLK_SAMPLE_RISING |
 +		V4L2_MBUS_PCLK_SAMPLE_FALLING;
 +
 +	/* If requested data width is supported by the platform, use it */
 +	if ((1 << (buswidth - 1)) & pcdev->width_flags)
 +		return 0;
 +
 +	return -EINVAL;
 +}
 +
 +static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
 +				  unsigned long flags, __u32 pixfmt)
 +{
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct pxa_camera_dev *pcdev = ici->priv;
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	unsigned long dw, bpp;
 +	u32 cicr0, cicr1, cicr2, cicr3, cicr4 = 0, y_skip_top;
 +	int ret = v4l2_subdev_call(sd, sensor, g_skip_top_lines, &y_skip_top);
 +
 +	if (ret < 0)
 +		y_skip_top = 0;
 +
 +	/*
 +	 * Datawidth is now guaranteed to be equal to one of the three values.
 +	 * We fix bit-per-pixel equal to data-width...
 +	 */
 +	switch (icd->current_fmt->host_fmt->bits_per_sample) {
 +	case 10:
 +		dw = 4;
 +		bpp = 0x40;
 +		break;
 +	case 9:
 +		dw = 3;
 +		bpp = 0x20;
 +		break;
 +	default:
 +		/*
 +		 * Actually it can only be 8 now,
 +		 * default is just to silence compiler warnings
 +		 */
 +	case 8:
 +		dw = 2;
 +		bpp = 0;
 +	}
 +
 +	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
 +		cicr4 |= CICR4_PCLK_EN;
 +	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
 +		cicr4 |= CICR4_MCLK_EN;
 +	if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
 +		cicr4 |= CICR4_PCP;
 +	if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
 +		cicr4 |= CICR4_HSP;
 +	if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
 +		cicr4 |= CICR4_VSP;
 +
 +	cicr0 = __raw_readl(pcdev->base + CICR0);
 +	if (cicr0 & CICR0_ENB)
 +		__raw_writel(cicr0 & ~CICR0_ENB, pcdev->base + CICR0);
 +
 +	cicr1 = CICR1_PPL_VAL(icd->user_width - 1) | bpp | dw;
 +
 +	switch (pixfmt) {
 +	case V4L2_PIX_FMT_YUV422P:
 +		pcdev->channels = 3;
 +		cicr1 |= CICR1_YCBCR_F;
 +		/*
 +		 * Normally, pxa bus wants as input UYVY format. We allow all
 +		 * reorderings of the YUV422 format, as no processing is done,
 +		 * and the YUV stream is just passed through without any
 +		 * transformation. Note that UYVY is the only format that
 +		 * should be used if pxa framebuffer Overlay2 is used.
 +		 */
 +	case V4L2_PIX_FMT_UYVY:
 +	case V4L2_PIX_FMT_VYUY:
 +	case V4L2_PIX_FMT_YUYV:
 +	case V4L2_PIX_FMT_YVYU:
 +		cicr1 |= CICR1_COLOR_SP_VAL(2);
 +		break;
 +	case V4L2_PIX_FMT_RGB555:
 +		cicr1 |= CICR1_RGB_BPP_VAL(1) | CICR1_RGBT_CONV_VAL(2) |
 +			CICR1_TBIT | CICR1_COLOR_SP_VAL(1);
 +		break;
 +	case V4L2_PIX_FMT_RGB565:
 +		cicr1 |= CICR1_COLOR_SP_VAL(1) | CICR1_RGB_BPP_VAL(2);
 +		break;
 +	}
 +
 +	cicr2 = 0;
 +	cicr3 = CICR3_LPF_VAL(icd->user_height - 1) |
 +		CICR3_BFW_VAL(min((u32)255, y_skip_top));
 +	cicr4 |= pcdev->mclk_divisor;
 +
 +	__raw_writel(cicr1, pcdev->base + CICR1);
 +	__raw_writel(cicr2, pcdev->base + CICR2);
 +	__raw_writel(cicr3, pcdev->base + CICR3);
 +	__raw_writel(cicr4, pcdev->base + CICR4);
 +
 +	/* CIF interrupts are not used, only DMA */
 +	cicr0 = (cicr0 & CICR0_ENB) | (pcdev->platform_flags & PXA_CAMERA_MASTER ?
 +		CICR0_SIM_MP : (CICR0_SL_CAP_EN | CICR0_SIM_SP));
 +	cicr0 |= CICR0_DMAEN | CICR0_IRQ_MASK;
 +	__raw_writel(cicr0, pcdev->base + CICR0);
 +}
 +
 +static int pxa_camera_set_bus_param(struct soc_camera_device *icd)
 +{
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct pxa_camera_dev *pcdev = ici->priv;
 +	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
 +	u32 pixfmt = icd->current_fmt->host_fmt->fourcc;
 +	unsigned long bus_flags, common_flags;
 +	int ret;
 +	struct pxa_cam *cam = icd->host_priv;
 +
 +	ret = test_platform_param(pcdev, icd->current_fmt->host_fmt->bits_per_sample,
 +				  &bus_flags);
 +	if (ret < 0)
 +		return ret;
 +
 +	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
 +	if (!ret) {
 +		common_flags = soc_mbus_config_compatible(&cfg,
 +							  bus_flags);
 +		if (!common_flags) {
 +			dev_warn(icd->parent,
 +				 "Flags incompatible: camera 0x%x, host 0x%lx\n",
 +				 cfg.flags, bus_flags);
 +			return -EINVAL;
 +		}
 +	} else if (ret != -ENOIOCTLCMD) {
 +		return ret;
 +	} else {
 +		common_flags = bus_flags;
 +	}
 +
 +	pcdev->channels = 1;
 +
 +	/* Make choises, based on platform preferences */
 +	if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
 +	    (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
 +		if (pcdev->platform_flags & PXA_CAMERA_HSP)
 +			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
 +		else
 +			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
 +	}
 +
 +	if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
 +	    (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
 +		if (pcdev->platform_flags & PXA_CAMERA_VSP)
 +			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
 +		else
 +			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
 +	}
 +
 +	if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
 +	    (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
 +		if (pcdev->platform_flags & PXA_CAMERA_PCP)
 +			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
 +		else
 +			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
 +	}
 +
 +	cfg.flags = common_flags;
 +	ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
 +	if (ret < 0 && ret != -ENOIOCTLCMD) {
 +		dev_dbg(icd->parent, "camera s_mbus_config(0x%lx) returned %d\n",
 +			common_flags, ret);
 +		return ret;
 +	}
 +
 +	cam->flags = common_flags;
 +
 +	pxa_camera_setup_cicr(icd, common_flags, pixfmt);
 +
 +	return 0;
 +}
 +
 +static int pxa_camera_try_bus_param(struct soc_camera_device *icd,
 +				    unsigned char buswidth)
 +{
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 +	struct pxa_camera_dev *pcdev = ici->priv;
 +	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
 +	unsigned long bus_flags, common_flags;
 +	int ret = test_platform_param(pcdev, buswidth, &bus_flags);
 +
 +	if (ret < 0)
 +		return ret;
 +
 +	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
 +	if (!ret) {
 +		common_flags = soc_mbus_config_compatible(&cfg,
 +							  bus_flags);
 +		if (!common_flags) {
 +			dev_warn(icd->parent,
 +				 "Flags incompatible: camera 0x%x, host 0x%lx\n",
 +				 cfg.flags, bus_flags);
 +			return -EINVAL;
 +		}
 +	} else if (ret == -ENOIOCTLCMD) {
 +		ret = 0;
 +	}
 +
 +	return ret;
 +}
 +
 +static const struct soc_mbus_pixelfmt pxa_camera_formats[] = {
 +	{
 +		.fourcc			= V4L2_PIX_FMT_YUV422P,
 +		.name			= "Planar YUV422 16 bit",
 +		.bits_per_sample	= 8,
 +		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 +		.order			= SOC_MBUS_ORDER_LE,
 +		.layout			= SOC_MBUS_LAYOUT_PLANAR_2Y_U_V,
 +	},
 +};
 +
 +/* This will be corrected as we get more formats */
 +static bool pxa_camera_packing_supported(const struct soc_mbus_pixelfmt *fmt)
 +{
 +	return	fmt->packing == SOC_MBUS_PACKING_NONE ||
 +		(fmt->bits_per_sample == 8 &&
 +		 fmt->packing == SOC_MBUS_PACKING_2X8_PADHI) ||
 +		(fmt->bits_per_sample > 8 &&
 +		 fmt->packing == SOC_MBUS_PACKING_EXTEND16);
 +}
 +
 +static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int idx,
 +				  struct soc_camera_format_xlate *xlate)
 +{
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	struct device *dev = icd->parent;
 +	int formats = 0, ret;
 +	struct pxa_cam *cam;
 +	enum v4l2_mbus_pixelcode code;
 +	const struct soc_mbus_pixelfmt *fmt;
 +
 +	ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
 +	if (ret < 0)
 +		/* No more formats */
 +		return 0;
 +
 +	fmt = soc_mbus_get_fmtdesc(code);
 +	if (!fmt) {
 +		dev_err(dev, "Invalid format code #%u: %d\n", idx, code);
 +		return 0;
 +	}
 +
 +	/* This also checks support for the requested bits-per-sample */
 +	ret = pxa_camera_try_bus_param(icd, fmt->bits_per_sample);
 +	if (ret < 0)
 +		return 0;
 +
 +	if (!icd->host_priv) {
 +		cam = kzalloc(sizeof(*cam), GFP_KERNEL);
 +		if (!cam)
 +			return -ENOMEM;
 +
 +		icd->host_priv = cam;
 +	} else {
 +		cam = icd->host_priv;
 +	}
 +
 +	switch (code) {
 +	case V4L2_MBUS_FMT_UYVY8_2X8:
 +		formats++;
 +		if (xlate) {
 +			xlate->host_fmt	= &pxa_camera_formats[0];
 +			xlate->code	= code;
 +			xlate++;
 +			dev_dbg(dev, "Providing format %s using code %d\n",
 +				pxa_camera_formats[0].name, code);
 +		}
 +	case V4L2_MBUS_FMT_VYUY8_2X8:
 +	case V4L2_MBUS_FMT_YUYV8_2X8:
 +	case V4L2_MBUS_FMT_YVYU8_2X8:
 +	case V4L2_MBUS_FMT_RGB565_2X8_LE:
 +	case V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE:
 +		if (xlate)
 +			dev_dbg(dev, "Providing format %s packed\n",
 +				fmt->name);
 +		break;
 +	default:
 +		if (!pxa_camera_packing_supported(fmt))
 +			return 0;
 +		if (xlate)
 +			dev_dbg(dev,
 +				"Providing format %s in pass-through mode\n",
 +				fmt->name);
 +	}
 +
 +	/* Generic pass-through */
 +	formats++;
 +	if (xlate) {
 +		xlate->host_fmt	= fmt;
 +		xlate->code	= code;
 +		xlate++;
 +	}
 +
 +	return formats;
 +}
 +
 +static void pxa_camera_put_formats(struct soc_camera_device *icd)
 +{
 +	kfree(icd->host_priv);
 +	icd->host_priv = NULL;
 +}
 +
 +static int pxa_camera_check_frame(u32 width, u32 height)
 +{
 +	/* limit to pxa hardware capabilities */
 +	return height < 32 || height > 2048 || width < 48 || width > 2048 ||
 +		(width & 0x01);
 +}
 +
 +static int pxa_camera_set_crop(struct soc_camera_device *icd,
 +			       struct v4l2_crop *a)
 +{
 +	struct v4l2_rect *rect = &a->c;
 +	struct device *dev = icd->parent;
 +	struct soc_camera_host *ici = to_soc_camera_host(dev);
 +	struct pxa_camera_dev *pcdev = ici->priv;
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	struct soc_camera_sense sense = {
 +		.master_clock = pcdev->mclk,
 +		.pixel_clock_max = pcdev->ciclk / 4,
 +	};
 +	struct v4l2_mbus_framefmt mf;
 +	struct pxa_cam *cam = icd->host_priv;
 +	u32 fourcc = icd->current_fmt->host_fmt->fourcc;
 +	int ret;
 +
 +	/* If PCLK is used to latch data from the sensor, check sense */
 +	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
 +		icd->sense = &sense;
 +
 +	ret = v4l2_subdev_call(sd, video, s_crop, a);
 +
 +	icd->sense = NULL;
 +
 +	if (ret < 0) {
 +		dev_warn(dev, "Failed to crop to %ux%u@%u:%u\n",
 +			 rect->width, rect->height, rect->left, rect->top);
 +		return ret;
 +	}
 +
 +	ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
 +	if (ret < 0)
 +		return ret;
 +
 +	if (pxa_camera_check_frame(mf.width, mf.height)) {
 +		/*
 +		 * Camera cropping produced a frame beyond our capabilities.
 +		 * FIXME: just extract a subframe, that we can process.
 +		 */
 +		v4l_bound_align_image(&mf.width, 48, 2048, 1,
 +			&mf.height, 32, 2048, 0,
 +			fourcc == V4L2_PIX_FMT_YUV422P ? 4 : 0);
 +		ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
 +		if (ret < 0)
 +			return ret;
 +
 +		if (pxa_camera_check_frame(mf.width, mf.height)) {
 +			dev_warn(icd->parent,
 +				 "Inconsistent state. Use S_FMT to repair\n");
 +			return -EINVAL;
 +		}
 +	}
 +
 +	if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {
 +		if (sense.pixel_clock > sense.pixel_clock_max) {
 +			dev_err(dev,
 +				"pixel clock %lu set by the camera too high!",
 +				sense.pixel_clock);
 +			return -EIO;
 +		}
 +		recalculate_fifo_timeout(pcdev, sense.pixel_clock);
 +	}
 +
 +	icd->user_width		= mf.width;
 +	icd->user_height	= mf.height;
 +
 +	pxa_camera_setup_cicr(icd, cam->flags, fourcc);
 +
 +	return ret;
 +}
 +
 +static int pxa_camera_set_fmt(struct soc_camera_device *icd,
 +			      struct v4l2_format *f)
 +{
 +	struct device *dev = icd->parent;
 +	struct soc_camera_host *ici = to_soc_camera_host(dev);
 +	struct pxa_camera_dev *pcdev = ici->priv;
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	const struct soc_camera_format_xlate *xlate = NULL;
 +	struct soc_camera_sense sense = {
 +		.master_clock = pcdev->mclk,
 +		.pixel_clock_max = pcdev->ciclk / 4,
 +	};
 +	struct v4l2_pix_format *pix = &f->fmt.pix;
 +	struct v4l2_mbus_framefmt mf;
 +	int ret;
 +
 +	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
 +	if (!xlate) {
 +		dev_warn(dev, "Format %x not found\n", pix->pixelformat);
 +		return -EINVAL;
 +	}
 +
 +	/* If PCLK is used to latch data from the sensor, check sense */
 +	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
 +		/* The caller holds a mutex. */
 +		icd->sense = &sense;
 +
 +	mf.width	= pix->width;
 +	mf.height	= pix->height;
 +	mf.field	= pix->field;
 +	mf.colorspace	= pix->colorspace;
 +	mf.code		= xlate->code;
 +
 +	ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
 +
 +	if (mf.code != xlate->code)
 +		return -EINVAL;
 +
 +	icd->sense = NULL;
 +
 +	if (ret < 0) {
 +		dev_warn(dev, "Failed to configure for format %x\n",
 +			 pix->pixelformat);
 +	} else if (pxa_camera_check_frame(mf.width, mf.height)) {
 +		dev_warn(dev,
 +			 "Camera driver produced an unsupported frame %dx%d\n",
 +			 mf.width, mf.height);
 +		ret = -EINVAL;
 +	} else if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {
 +		if (sense.pixel_clock > sense.pixel_clock_max) {
 +			dev_err(dev,
 +				"pixel clock %lu set by the camera too high!",
 +				sense.pixel_clock);
 +			return -EIO;
 +		}
 +		recalculate_fifo_timeout(pcdev, sense.pixel_clock);
 +	}
 +
 +	if (ret < 0)
 +		return ret;
 +
 +	pix->width		= mf.width;
 +	pix->height		= mf.height;
 +	pix->field		= mf.field;
 +	pix->colorspace		= mf.colorspace;
 +	icd->current_fmt	= xlate;
 +
 +	return ret;
 +}
 +
 +static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 +			      struct v4l2_format *f)
 +{
 +	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 +	const struct soc_camera_format_xlate *xlate;
 +	struct v4l2_pix_format *pix = &f->fmt.pix;
 +	struct v4l2_mbus_framefmt mf;
 +	__u32 pixfmt = pix->pixelformat;
 +	int ret;
 +
 +	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
 +	if (!xlate) {
 +		dev_warn(icd->parent, "Format %x not found\n", pixfmt);
 +		return -EINVAL;
 +	}
 +
 +	/*
 +	 * Limit to pxa hardware capabilities.  YUV422P planar format requires
 +	 * images size to be a multiple of 16 bytes.  If not, zeros will be
 +	 * inserted between Y and U planes, and U and V planes, which violates
 +	 * the YUV422P standard.
 +	 */
 +	v4l_bound_align_image(&pix->width, 48, 2048, 1,
 +			      &pix->height, 32, 2048, 0,
 +			      pixfmt == V4L2_PIX_FMT_YUV422P ? 4 : 0);
 +
 +	/* limit to sensor capabilities */
 +	mf.width	= pix->width;
 +	mf.height	= pix->height;
 +	/* Only progressive video supported so far */
 +	mf.field	= V4L2_FIELD_NONE;
 +	mf.colorspace	= pix->colorspace;
 +	mf.code		= xlate->code;
 +
 +	ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
 +	if (ret < 0)
 +		return ret;
 +
 +	pix->width	= mf.width;
 +	pix->height	= mf.height;
 +	pix->colorspace	= mf.colorspace;
 +
 +	switch (mf.field) {
 +	case V4L2_FIELD_ANY:
 +	case V4L2_FIELD_NONE:
 +		pix->field	= V4L2_FIELD_NONE;
 +		break;
 +	default:
 +		/* TODO: support interlaced at least in pass-through mode */
 +		dev_err(icd->parent, "Field type %d unsupported.\n",
 +			mf.field);
 +		return -EINVAL;
 +	}
 +
 +	return ret;
 +}
 +
 +static int pxa_camera_reqbufs(struct soc_camera_device *icd,
 +			      struct v4l2_requestbuffers *p)
 +{
 +	int i;
 +
 +	/*
 +	 * This is for locking debugging only. I removed spinlocks and now I
 +	 * check whether .prepare is ever called on a linked buffer, or whether
 +	 * a dma IRQ can occur for an in-work or unlinked buffer. Until now
 +	 * it hadn't triggered
 +	 */
 +	for (i = 0; i < p->count; i++) {
 +		struct pxa_buffer *buf = container_of(icd->vb_vidq.bufs[i],
 +						      struct pxa_buffer, vb);
 +		buf->inwork = 0;
 +		INIT_LIST_HEAD(&buf->vb.queue);
 +	}
 +
 +	return 0;
 +}
 +
 +static unsigned int pxa_camera_poll(struct file *file, poll_table *pt)
 +{
 +	struct soc_camera_device *icd = file->private_data;
 +	struct pxa_buffer *buf;
 +
 +	buf = list_entry(icd->vb_vidq.stream.next, struct pxa_buffer,
 +			 vb.stream);
 +
 +	poll_wait(file, &buf->vb.done, pt);
 +
 +	if (buf->vb.state == VIDEOBUF_DONE ||
 +	    buf->vb.state == VIDEOBUF_ERROR)
 +		return POLLIN|POLLRDNORM;
 +
 +	return 0;
 +}
 +
 +static int pxa_camera_querycap(struct soc_camera_host *ici,
 +			       struct v4l2_capability *cap)
 +{
 +	/* cap->name is set by the firendly caller:-> */
 +	strlcpy(cap->card, pxa_cam_driver_description, sizeof(cap->card));
 +	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 +
 +	return 0;
 +}
 +
 +static int pxa_camera_suspend(struct device *dev)
 +{
 +	struct soc_camera_host *ici = to_soc_camera_host(dev);
 +	struct pxa_camera_dev *pcdev = ici->priv;
 +	int i = 0, ret = 0;
 +
 +	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR0);
 +	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR1);
 +	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR2);
 +	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR3);
 +	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR4);
 +
 +	if (pcdev->icd) {
 +		struct v4l2_subdev *sd = soc_camera_to_subdev(pcdev->icd);
 +		ret = v4l2_subdev_call(sd, core, s_power, 0);
 +		if (ret == -ENOIOCTLCMD)
 +			ret = 0;
 +	}
 +
 +	return ret;
 +}
 +
 +static int pxa_camera_resume(struct device *dev)
 +{
 +	struct soc_camera_host *ici = to_soc_camera_host(dev);
 +	struct pxa_camera_dev *pcdev = ici->priv;
 +	int i = 0, ret = 0;
 +
 +	DRCMR(68) = pcdev->dma_chans[0] | DRCMR_MAPVLD;
 +	DRCMR(69) = pcdev->dma_chans[1] | DRCMR_MAPVLD;
 +	DRCMR(70) = pcdev->dma_chans[2] | DRCMR_MAPVLD;
 +
 +	__raw_writel(pcdev->save_cicr[i++] & ~CICR0_ENB, pcdev->base + CICR0);
 +	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR1);
 +	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR2);
 +	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR3);
 +	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR4);
 +
 +	if (pcdev->icd) {
 +		struct v4l2_subdev *sd = soc_camera_to_subdev(pcdev->icd);
 +		ret = v4l2_subdev_call(sd, core, s_power, 1);
 +		if (ret == -ENOIOCTLCMD)
 +			ret = 0;
 +	}
 +
 +	/* Restart frame capture if active buffer exists */
 +	if (!ret && pcdev->active)
 +		pxa_camera_start_capture(pcdev);
 +
 +	return ret;
 +}
 +
 +static struct soc_camera_host_ops pxa_soc_camera_host_ops = {
 +	.owner		= THIS_MODULE,
 +	.add		= pxa_camera_add_device,
 +	.remove		= pxa_camera_remove_device,
 +	.set_crop	= pxa_camera_set_crop,
 +	.get_formats	= pxa_camera_get_formats,
 +	.put_formats	= pxa_camera_put_formats,
 +	.set_fmt	= pxa_camera_set_fmt,
 +	.try_fmt	= pxa_camera_try_fmt,
 +	.init_videobuf	= pxa_camera_init_videobuf,
 +	.reqbufs	= pxa_camera_reqbufs,
 +	.poll		= pxa_camera_poll,
 +	.querycap	= pxa_camera_querycap,
 +	.set_bus_param	= pxa_camera_set_bus_param,
 +};
 +
 +static int __devinit pxa_camera_probe(struct platform_device *pdev)
 +{
 +	struct pxa_camera_dev *pcdev;
 +	struct resource *res;
 +	void __iomem *base;
 +	int irq;
 +	int err = 0;
 +
 +	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 +	irq = platform_get_irq(pdev, 0);
 +	if (!res || irq < 0) {
 +		err = -ENODEV;
 +		goto exit;
 +	}
 +
 +	pcdev = kzalloc(sizeof(*pcdev), GFP_KERNEL);
 +	if (!pcdev) {
 +		dev_err(&pdev->dev, "Could not allocate pcdev\n");
 +		err = -ENOMEM;
 +		goto exit;
 +	}
 +
 +	pcdev->clk = clk_get(&pdev->dev, NULL);
 +	if (IS_ERR(pcdev->clk)) {
 +		err = PTR_ERR(pcdev->clk);
 +		goto exit_kfree;
 +	}
 +
 +	pcdev->res = res;
 +
 +	pcdev->pdata = pdev->dev.platform_data;
 +	pcdev->platform_flags = pcdev->pdata->flags;
 +	if (!(pcdev->platform_flags & (PXA_CAMERA_DATAWIDTH_8 |
 +			PXA_CAMERA_DATAWIDTH_9 | PXA_CAMERA_DATAWIDTH_10))) {
 +		/*
 +		 * Platform hasn't set available data widths. This is bad.
 +		 * Warn and use a default.
 +		 */
 +		dev_warn(&pdev->dev, "WARNING! Platform hasn't set available "
 +			 "data widths, using default 10 bit\n");
 +		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_10;
 +	}
 +	if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_8)
 +		pcdev->width_flags = 1 << 7;
 +	if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_9)
 +		pcdev->width_flags |= 1 << 8;
 +	if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_10)
 +		pcdev->width_flags |= 1 << 9;
 +	pcdev->mclk = pcdev->pdata->mclk_10khz * 10000;
 +	if (!pcdev->mclk) {
 +		dev_warn(&pdev->dev,
 +			 "mclk == 0! Please, fix your platform data. "
 +			 "Using default 20MHz\n");
 +		pcdev->mclk = 20000000;
 +	}
 +
 +	pcdev->mclk_divisor = mclk_get_divisor(pdev, pcdev);
 +
 +	INIT_LIST_HEAD(&pcdev->capture);
 +	spin_lock_init(&pcdev->lock);
 +
 +	/*
 +	 * Request the regions.
 +	 */
 +	if (!request_mem_region(res->start, resource_size(res),
 +				PXA_CAM_DRV_NAME)) {
 +		err = -EBUSY;
 +		goto exit_clk;
 +	}
 +
 +	base = ioremap(res->start, resource_size(res));
 +	if (!base) {
 +		err = -ENOMEM;
 +		goto exit_release;
 +	}
 +	pcdev->irq = irq;
 +	pcdev->base = base;
 +
 +	/* request dma */
 +	err = pxa_request_dma("CI_Y", DMA_PRIO_HIGH,
 +			      pxa_camera_dma_irq_y, pcdev);
 +	if (err < 0) {
 +		dev_err(&pdev->dev, "Can't request DMA for Y\n");
 +		goto exit_iounmap;
 +	}
 +	pcdev->dma_chans[0] = err;
 +	dev_dbg(&pdev->dev, "got DMA channel %d\n", pcdev->dma_chans[0]);
 +
 +	err = pxa_request_dma("CI_U", DMA_PRIO_HIGH,
 +			      pxa_camera_dma_irq_u, pcdev);
 +	if (err < 0) {
 +		dev_err(&pdev->dev, "Can't request DMA for U\n");
 +		goto exit_free_dma_y;
 +	}
 +	pcdev->dma_chans[1] = err;
 +	dev_dbg(&pdev->dev, "got DMA channel (U) %d\n", pcdev->dma_chans[1]);
 +
 +	err = pxa_request_dma("CI_V", DMA_PRIO_HIGH,
 +			      pxa_camera_dma_irq_v, pcdev);
 +	if (err < 0) {
 +		dev_err(&pdev->dev, "Can't request DMA for V\n");
 +		goto exit_free_dma_u;
 +	}
 +	pcdev->dma_chans[2] = err;
 +	dev_dbg(&pdev->dev, "got DMA channel (V) %d\n", pcdev->dma_chans[2]);
 +
 +	DRCMR(68) = pcdev->dma_chans[0] | DRCMR_MAPVLD;
 +	DRCMR(69) = pcdev->dma_chans[1] | DRCMR_MAPVLD;
 +	DRCMR(70) = pcdev->dma_chans[2] | DRCMR_MAPVLD;
 +
 +	/* request irq */
 +	err = request_irq(pcdev->irq, pxa_camera_irq, 0, PXA_CAM_DRV_NAME,
 +			  pcdev);
 +	if (err) {
 +		dev_err(&pdev->dev, "Camera interrupt register failed \n");
 +		goto exit_free_dma;
 +	}
 +
 +	pcdev->soc_host.drv_name	= PXA_CAM_DRV_NAME;
 +	pcdev->soc_host.ops		= &pxa_soc_camera_host_ops;
 +	pcdev->soc_host.priv		= pcdev;
 +	pcdev->soc_host.v4l2_dev.dev	= &pdev->dev;
 +	pcdev->soc_host.nr		= pdev->id;
 +
 +	err = soc_camera_host_register(&pcdev->soc_host);
 +	if (err)
 +		goto exit_free_irq;
 +
 +	return 0;
 +
 +exit_free_irq:
 +	free_irq(pcdev->irq, pcdev);
 +exit_free_dma:
 +	pxa_free_dma(pcdev->dma_chans[2]);
 +exit_free_dma_u:
 +	pxa_free_dma(pcdev->dma_chans[1]);
 +exit_free_dma_y:
 +	pxa_free_dma(pcdev->dma_chans[0]);
 +exit_iounmap:
 +	iounmap(base);
 +exit_release:
 +	release_mem_region(res->start, resource_size(res));
 +exit_clk:
 +	clk_put(pcdev->clk);
 +exit_kfree:
 +	kfree(pcdev);
 +exit:
 +	return err;
 +}
 +
 +static int __devexit pxa_camera_remove(struct platform_device *pdev)
 +{
 +	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
 +	struct pxa_camera_dev *pcdev = container_of(soc_host,
 +					struct pxa_camera_dev, soc_host);
 +	struct resource *res;
 +
 +	clk_put(pcdev->clk);
 +
 +	pxa_free_dma(pcdev->dma_chans[0]);
 +	pxa_free_dma(pcdev->dma_chans[1]);
 +	pxa_free_dma(pcdev->dma_chans[2]);
 +	free_irq(pcdev->irq, pcdev);
 +
 +	soc_camera_host_unregister(soc_host);
 +
 +	iounmap(pcdev->base);
 +
 +	res = pcdev->res;
 +	release_mem_region(res->start, resource_size(res));
 +
 +	kfree(pcdev);
 +
 +	dev_info(&pdev->dev, "PXA Camera driver unloaded\n");
 +
 +	return 0;
 +}
 +
 +static struct dev_pm_ops pxa_camera_pm = {
 +	.suspend	= pxa_camera_suspend,
 +	.resume		= pxa_camera_resume,
 +};
 +
 +static struct platform_driver pxa_camera_driver = {
 +	.driver 	= {
 +		.name	= PXA_CAM_DRV_NAME,
 +		.pm	= &pxa_camera_pm,
 +	},
 +	.probe		= pxa_camera_probe,
 +	.remove		= __devexit_p(pxa_camera_remove),
 +};
 +
 +module_platform_driver(pxa_camera_driver);
 +
 +MODULE_DESCRIPTION("PXA27x SoC Camera Host driver");
 +MODULE_AUTHOR("Guennadi Liakhovetski <kernel@pengutronix.de>");
 +MODULE_LICENSE("GPL");
 +MODULE_VERSION(PXA_CAM_VERSION);
 +MODULE_ALIAS("platform:" PXA_CAM_DRV_NAME);