Merge branch 'samsung_platform_data' into staging/for_v3.7
authorMauro Carvalho Chehab <mchehab@redhat.com>
Sat, 6 Oct 2012 01:32:05 +0000 (22:32 -0300)
committerMauro Carvalho Chehab <mchehab@redhat.com>
Sat, 6 Oct 2012 01:32:05 +0000 (22:32 -0300)
* 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

27 files changed:
1  2 
arch/arm/mach-at91/at91sam9g45_devices.c
arch/arm/mach-at91/at91sam9rl_devices.c
arch/arm/mach-davinci/board-da850-evm.c
arch/arm/mach-davinci/board-dm644x-evm.c
arch/arm/mach-davinci/board-dm646x-evm.c
arch/arm/mach-davinci/include/mach/da8xx.h
arch/arm/mach-dove/common.c
arch/arm/mach-exynos/mach-nuri.c
arch/arm/mach-exynos/mach-origen.c
arch/arm/mach-exynos/mach-smdkv310.c
arch/arm/mach-exynos/mach-universal_c210.c
arch/arm/mach-kirkwood/common.c
arch/arm/mach-kirkwood/db88f6281-bp-setup.c
arch/arm/mach-mmp/sram.c
arch/arm/mach-mv78xx0/common.c
arch/arm/mach-orion5x/common.c
arch/arm/mach-ux500/board-mop500.c
arch/arm/plat-mxc/include/mach/devices-common.h
arch/arm/plat-orion/common.c
arch/arm/plat-samsung/devs.c
drivers/media/platform/davinci/vpbe_venc.c
drivers/media/platform/s5p-fimc/mipi-csis.c
drivers/media/platform/soc_camera/mx1_camera.c
drivers/media/platform/soc_camera/mx2_camera.c
drivers/media/platform/soc_camera/mx3_camera.c
drivers/media/platform/soc_camera/pxa_camera.c
drivers/tty/serial/imx.c

index bc6b8c7b86c33e8ed8587c63d3f3fc1dbb37a1ed,1295e616ceee7e32a5b31d62b3cf52a4286578b3..32ee3f89596738d988a00105c0358d4ff1a61f1b
  
  #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)
index 8eecbce7febc08a64e0b89d6fc15fe8af616652e,33e78ae2a254d3106f97a42dcf68aa2cebc076c2..13d229575757ffadcce3636068c03512d04fa948
  
  #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;
  
Simple merge
Simple merge
index a54ce4ef7057c765db83f0217fa65ccff09ec884,4a531dccd0c6f175efbbbd09c97d6849d30fc2de..0b8d24e27e74d45eeff900341b62808673dd066e
@@@ -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>
index 73f2bce097e179822d9a08d4dc14bba6f104888c,fa2ada6c9ca7d09bef0b4256c5f14ce1985748d0..1b864ee387cd70a73e65f93fb74058891c0ed273
  #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"
Simple merge
Simple merge
Simple merge
Simple merge
Simple merge
Simple merge
Simple merge
index 86d47c39033d90d20267423f9d3dd57449117d0b,0000000000000000000000000000000000000000..aed7369b962a7da5a5c7e6c826ce06944d12136c
mode 100644,000000..100644
--- /dev/null
@@@ -1,707 -1,0 +1,707 @@@
- #include <mach/i2c.h>
 +/*
 + * 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 <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");
index 3438ccf3c332511b7a8403c0a938b97bccfde8c8,0000000000000000000000000000000000000000..22bdf211a2ac8544f6b5a25dfffe9cd9b5bb5909
mode 100644,000000..100644
--- /dev/null
@@@ -1,888 -1,0 +1,888 @@@
- #include <plat/mipi_csis.h>
 +/*
 + * 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 <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");
index 560a65aa7038dd3cded8d08395958c70e3c537bb,0000000000000000000000000000000000000000..bbe70991d30b6ac0b2ef598ccafba7519ab138b9
mode 100644,000000..100644
--- /dev/null
@@@ -1,889 -1,0 +1,889 @@@
- #include <mach/mx1_camera.h>
 +/*
 + * 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 <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);
index 619df35de3da18d0691c2ea1438699b952cdc72e,0000000000000000000000000000000000000000..403d7f17bfab1277adba82ed59d0650f99dd16aa
mode 100644,000000..100644
--- /dev/null
@@@ -1,1863 -1,0 +1,1863 @@@
- #include <mach/mx2_cam.h>
 +/*
 + * 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 <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);
index 16975c6e69057f7799baf568a589d700d623a7a7,0000000000000000000000000000000000000000..3557ac97e4303f4743330850a5144f43cb06cbd0
mode 100644,000000..100644
--- /dev/null
@@@ -1,1290 -1,0 +1,1290 @@@
- #include <mach/mx3_camera.h>
- #include <mach/dma.h>
 +/*
 + * 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 <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);
index 9c21e01f2c24c9145cc3d7287c9f2db394eb7c8f,0000000000000000000000000000000000000000..1e3776d08dacb6f6159a417ecbe5d6bf9c935f3f
mode 100644,000000..100644
--- /dev/null
@@@ -1,1852 -1,0 +1,1852 @@@
- #include <mach/camera.h>
 +/*
 + * 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 <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);
Simple merge