diff --git a/drivers/media/platform/soc_camera/Kconfig b/drivers/media/platform/soc_camera/Kconfig
index 9afe1e7..4d6c74d 100644
--- a/drivers/media/platform/soc_camera/Kconfig
+++ b/drivers/media/platform/soc_camera/Kconfig
@@ -85,3 +85,10 @@ config VIDEO_ATMEL_ISI
 	  This module makes the ATMEL Image Sensor Interface available
 	  as a v4l2 device.
 
+config VIDEO_MPC52xx_CSI
+	tristate "IFM MPC5200 Camera Sensor Interface driver"
+	depends on SOC_CAMERA && PPC_MPC52xx
+	select PPC_BESTCOMM_GEN_BD
+	select VIDEOBUF2_DMA_CONTIG
+	---help---
+	  This is a driver for IFM camera sensor interface on mpc5200.
diff --git a/drivers/media/platform/soc_camera/Makefile b/drivers/media/platform/soc_camera/Makefile
index 136b7f8..79e6339 100644
--- a/drivers/media/platform/soc_camera/Makefile
+++ b/drivers/media/platform/soc_camera/Makefile
@@ -3,6 +3,7 @@ obj-$(CONFIG_SOC_CAMERA_PLATFORM)	+= soc_camera_platform.o
 
 # soc-camera host drivers have to be linked after camera drivers
 obj-$(CONFIG_VIDEO_ATMEL_ISI)		+= atmel-isi.o
+obj-$(CONFIG_VIDEO_MPC52xx_CSI)		+= mpc5200-csi-camera.o
 obj-$(CONFIG_VIDEO_MX1)			+= mx1_camera.o
 obj-$(CONFIG_VIDEO_MX2)			+= mx2_camera.o
 obj-$(CONFIG_VIDEO_MX3)			+= mx3_camera.o
diff --git a/drivers/media/platform/soc_camera/mpc5200-csi-camera.c b/drivers/media/platform/soc_camera/mpc5200-csi-camera.c
new file mode 100644
index 0000000..84746c1
--- /dev/null
+++ b/drivers/media/platform/soc_camera/mpc5200-csi-camera.c
@@ -0,0 +1,1212 @@
+/*
+ * Driver for image sensor/fpga interface on mpc5200 LPB found
+ * on IFM o2d based boards
+ *
+ * Code base taken from i.MX3x camera host driver
+ * Copyright (C) 2008
+ * Guennadi Liakhovetski, DENX Software Engineering, <lg@denx.de>
+ *
+ * Copyright (C) 2012
+ * Anatolij Gustschin <agust@denx.de>, DENX Software Engineering
+ *
+ * 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/kernel.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/unistd.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/gpio.h>
+#include <linux/of_gpio.h>
+#include <linux/dma-mapping.h>
+#include <asm/mpc52xx.h>
+
+#include <linux/i2c.h>
+#include <linux/videodev2.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>
+
+#define O2XXX_CAM_DRV_NAME	"o2d_csi"
+
+#define MAX_VIDEO_MEM		2	/* Video memory limit in megabytes */
+
+#define DEFAULT_IMAGE_WIDTH	688	/* Default image width */
+#define DEFAULT_IMAGE_HEIGHT	480	/* Default image height */
+#define DEFAULT_IMAGE_LENGTH	(DEFAULT_IMAGE_WIDTH * DEFAULT_IMAGE_HEIGHT)
+
+enum o2xxx_buffer_state {
+	BUF_NEEDS_INIT,
+	BUF_PREPARED,
+};
+
+struct o2xxx_camera_buffer {
+	/* v4l2_buffer must be first */
+	struct vb2_buffer	vb;
+	dma_addr_t		paddr;
+	size_t			bsize;
+	enum o2xxx_buffer_state	state;
+	struct list_head	queue;
+};
+
+struct o2d_csi_priv {
+	/* sensor control gpios */
+	int imag_capture;
+	int imag_reset;
+	int imag_master_en;
+
+	struct device_node *gpt_np;
+	struct mpc52xx_gpt __iomem *gpt;
+	struct mpc52xx_intr __iomem *intr;
+	int irq;
+	int chip_select;
+
+	/* LocalPlus Bus camera CS phys addr */
+	resource_size_t lpb_cs_phys;
+
+	resource_size_t lpbfifo_regs_phys;
+	void __iomem *lpbfifo_regs;
+
+	/* soc_camera and v4l2 interface */
+	struct platform_device *soc_cam_pdev;
+	struct soc_camera_device *icd;
+	struct soc_camera_host soc_host;
+	size_t buf_total;
+	int sequence;
+	struct vb2_alloc_ctx *alloc_ctx;
+
+	unsigned long image_length;
+
+	struct o2xxx_camera_buffer *active;
+	spinlock_t lock;	/* Protects video buffer lists */
+	struct list_head capture;
+	int streaming;
+	int smode;
+	int overrun;
+
+	struct i2c_client *client;
+	struct v4l2_subdev *fsubdev;
+
+	/* for sclpc fifo bestcomm */
+	struct mpc52xx_lpbfifo_request req;
+	int dmareq_queued;
+	int dma_running;
+};
+
+static int o2xxx_queue_dma_request(struct o2d_csi_priv *priv);
+static void o2xxx_bus_master_clock(struct o2d_csi_priv *priv, int onoff);
+
+static struct o2xxx_camera_buffer *to_o2xxx_vb(struct vb2_buffer *vb)
+{
+	return container_of(vb, struct o2xxx_camera_buffer, vb);
+}
+
+static struct i2c_board_info o2xxx_i2c_camera[] = {
+	{
+		I2C_BOARD_INFO("mt9v022", 0x5c),
+	},
+};
+
+static struct i2c_board_info o2xxx_ifm_camera[] = {
+	{
+		I2C_BOARD_INFO("ifm-fpga", 0x48),
+	},
+};
+
+static unsigned long o2xxx_query_bus_param(struct soc_camera_link *icl)
+{
+	pr_debug("%s()\n", __func__);
+
+	return SOCAM_DATAWIDTH_8;
+}
+
+static int o2xxx_add_subdevice(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct soc_camera_link *icl = to_soc_camera_link(icd);
+	struct i2c_adapter *adap = i2c_get_adapter(icl->i2c_adapter_id);
+	struct o2d_csi_priv *priv = ici->priv;
+	struct i2c_client *client;
+	struct v4l2_subdev *subdev;
+
+	dev_dbg(icd->parent, "%s()\n", __func__);
+
+	if (!adap) {
+		dev_err(icd->pdev, "Cannot get I2C adapter #%d. No driver?\n",
+			icl->i2c_adapter_id);
+		goto ei2cga;
+	}
+
+	o2xxx_i2c_camera[0].platform_data = icl;
+
+	subdev = v4l2_i2c_new_subdev_board(&ici->v4l2_dev, adap,
+				&o2xxx_i2c_camera[0], NULL);
+	if (!subdev)
+		goto ei2cnd;
+
+	client = v4l2_get_subdevdata(subdev);
+
+	/* Use to_i2c_client(dev) to recover the i2c client */
+	icd->control = &client->dev;
+
+	o2xxx_ifm_camera[0].platform_data = icl;
+	priv->fsubdev = v4l2_i2c_new_subdev_board(&ici->v4l2_dev, adap,
+					&o2xxx_ifm_camera[0], NULL);
+	if (priv->fsubdev) {
+		if (v4l2_ctrl_add_handler(&icd->ctrl_handler,
+					  priv->fsubdev->ctrl_handler)) {
+			dev_err(icd->parent, "fpga subdev ctrl hdl failed\n");
+			v4l2_device_unregister_subdev(priv->fsubdev);
+			i2c_unregister_device(v4l2_get_subdevdata(
+							priv->fsubdev));
+			goto ei2cnd;
+		}
+	} else {
+		dev_dbg(icd->parent, "fpga subdev not used\n");
+	}
+
+	dev_dbg(icd->parent, "%s() done\n", __func__);
+	return 0;
+ei2cnd:
+	i2c_put_adapter(adap);
+ei2cga:
+	return -ENODEV;
+}
+
+static void o2xxx_del_subdevice(struct soc_camera_device *icd)
+{
+	struct i2c_client *client =
+		to_i2c_client(to_soc_camera_control(icd));
+	struct i2c_adapter *adap = client->adapter;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct o2d_csi_priv *priv = ici->priv;
+
+	dev_dbg(icd->parent, "%s()\n", __func__);
+
+	if (priv->fsubdev) {
+		v4l2_device_unregister_subdev(priv->fsubdev);
+		i2c_unregister_device(v4l2_get_subdevdata(priv->fsubdev));
+	}
+
+	icd->control = NULL;
+	v4l2_device_unregister_subdev(i2c_get_clientdata(client));
+	i2c_unregister_device(client);
+	i2c_put_adapter(adap);
+}
+
+static struct soc_camera_link iclink_mt9v022 = {
+	.bus_id		= 0,		/* Must match with the camera ID */
+	.i2c_adapter_id	= 0,
+	.query_bus_param = o2xxx_query_bus_param,
+	.add_device	= o2xxx_add_subdevice,
+	.del_device	= o2xxx_del_subdevice,
+};
+
+static void o2xxx_sensor_reset(struct o2d_csi_priv *priv)
+{
+	gpio_set_value(priv->imag_reset, 0);
+	udelay(10);
+	gpio_set_value(priv->imag_reset, 1);
+}
+
+static void __maybe_unused o2xxx_sensor_capture(struct o2d_csi_priv *priv)
+{
+	udelay(3);
+	gpio_set_value(priv->imag_capture, 1);
+	udelay(3);
+	gpio_set_value(priv->imag_capture, 0);
+}
+
+static void o2xxx_dma_done_callback(struct mpc52xx_lpbfifo_request *req)
+{
+	struct o2d_csi_priv *priv = req->priv;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	dev_dbg(priv->icd->parent, "%s(): tb %lu\n", __func__, get_tbl());
+
+	priv->dma_running = 0;
+	priv->sequence++;
+
+	if (list_is_singular(&priv->capture)) {
+		/* not enough buffers in the queue, reuse last buffer */
+		o2xxx_queue_dma_request(priv);
+		priv->overrun++;
+		goto done;
+	}
+
+	if (list_empty(&priv->capture))
+		goto done;
+
+	if (priv->active) {
+		struct vb2_buffer *vb = &priv->active->vb;
+		struct o2xxx_camera_buffer *buf = priv->active;
+
+		list_del_init(&buf->queue);
+		do_gettimeofday(&vb->v4l2_buf.timestamp);
+		vb->v4l2_buf.sequence = priv->sequence;
+		vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+	}
+
+	/* queue next buffer */
+	priv->active = list_entry(priv->capture.next,
+				  struct o2xxx_camera_buffer, queue);
+	o2xxx_queue_dma_request(priv);
+
+done:
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static int o2xxx_queue_dma_request(struct o2d_csi_priv *priv)
+{
+	struct vb2_buffer *vb = &priv->active->vb;
+	struct o2xxx_camera_buffer *buf = to_o2xxx_vb(vb);
+	int ret;
+
+	priv->image_length = vb2_plane_size(vb, 0);
+
+	priv->req.cs = priv->chip_select;
+	priv->req.offset = 0;
+	priv->req.data_phys = buf->paddr;
+	priv->req.data = vb2_plane_vaddr(vb, 0);
+	priv->req.size = priv->image_length;
+	priv->req.pos = 0;
+	priv->req.defer_xfer_start = 1;
+	priv->req.flags = MPC52XX_LPBFIFO_FLAG_READ;
+	priv->req.callback = o2xxx_dma_done_callback;
+	priv->req.priv = priv;
+
+	ret = mpc52xx_lpbfifo_submit(&priv->req);
+	if (ret < 0) {
+		dev_err(priv->icd->parent,
+			"%s: submitting request failed: %d\n", __func__, ret);
+		return ret;
+	}
+
+	dev_dbg(priv->icd->parent, "%s: buf 0x%08x, sz %d\n",
+		__func__, (u32)priv->active->paddr, priv->req.size);
+	priv->dma_running = 0;
+	priv->dmareq_queued = 1;
+
+	return 0;
+}
+
+static int o2xxx_start_dma(struct o2d_csi_priv *priv)
+{
+	int ret;
+
+	/* start fifo transfer */
+	ret = mpc52xx_lpbfifo_start_xfer(&priv->req);
+	if (ret)
+		dev_err(priv->icd->parent, "can't start dma: %d\n", ret);
+
+	priv->dma_running = 1;
+
+	dev_dbg(priv->icd->parent, "%s: buf 0x%08x, tb %lu\n",
+		__func__, (u32)priv->active->paddr, get_tbl());
+	return 0;
+}
+
+/* sets the sysclk clock frequency of the image sensor */
+static int o2xxx_set_sysclk(struct o2d_csi_priv *priv, int int_speed, int on)
+{
+	u32 half_speed;
+	u32 reg;
+
+	/* SYSCLK (TIMER7), 132 / speed = SYSCLK MHz */
+	/* prescaler 1, int_speed counts */
+	half_speed = int_speed / 2;
+	iowrite32be(0x00010000 | (int_speed & 0xff), &priv->gpt->count);
+	iowrite32be(half_speed << 16, &priv->gpt->pwm);
+	iowrite32be((half_speed << 16) + 1, &priv->gpt->pwm); /* load */
+	if (on)
+		reg = 0x00000003;	/* module enable, PWM mode */
+	else
+		reg = 0x00000001;
+
+	iowrite32be(reg, &priv->gpt->mode);
+
+	return 0;
+}
+
+static int o2xxx_camera_add_device(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct o2d_csi_priv *priv = ici->priv;
+
+	if (priv->icd)
+		return -EBUSY;
+
+	priv->icd = icd;
+
+	dev_info(icd->parent, "Camera driver attached to camera %d\n",
+		 icd->devnum);
+
+	return 0;
+}
+
+static void o2xxx_camera_remove_device(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct o2d_csi_priv *priv = ici->priv;
+
+	dev_dbg(icd->parent, "%s()\n", __func__);
+
+	BUG_ON(icd != priv->icd);
+
+	priv->icd = NULL;
+
+	dev_info(icd->parent, "Camera driver detached from camera %d\n",
+		 icd->devnum);
+}
+
+static int o2xxx_camera_set_crop(struct soc_camera_device *icd,
+				 const struct v4l2_crop *a)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	int ret;
+
+	dev_dbg(icd->parent, "%s()\n", __func__);
+
+	ret = v4l2_subdev_call(sd, video, s_crop, a);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+
+static int o2xxx_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;
+
+	dev_dbg(icd->parent, "%s()\n", __func__);
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
+	if (!xlate) {
+		dev_warn(icd->parent, "Format %x not found\n",
+			 pix->pixelformat);
+		return -EINVAL;
+	}
+
+	dev_dbg(icd->parent, "Set format %dx%d\n", pix->width, pix->height);
+
+	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) {
+		dev_err(icd->parent, "sub s_mbus_fmt error\n");
+		return ret;
+	}
+
+	if (mf.code != xlate->code) {
+		dev_err(icd->parent, "code error\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int o2xxx_camera_try_fmt(struct soc_camera_device *icd,
+				struct v4l2_format *f)
+{
+	dev_dbg(icd->parent, "%s()\n", __func__);
+	return 0;
+}
+
+static const struct soc_mbus_pixelfmt o2xxx_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,
+	}, {
+		.fourcc			= V4L2_PIX_FMT_GREY,
+		.name			= "Monochrome 8 bit",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+};
+
+static int o2xxx_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;
+
+	dev_dbg(dev, "%s()\n", __func__);
+
+	ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
+	if (ret < 0)
+		return 0; /* No more formats */
+
+	fmt = soc_mbus_get_fmtdesc(code);
+	if (!fmt) {
+		dev_warn(icd->parent,
+			 "Unsupported format code #%u: %d\n", idx, code);
+		return 0;
+	}
+
+	switch (code) {
+	case V4L2_MBUS_FMT_SBGGR8_1X8:
+		formats++;
+		if (xlate) {
+			xlate->host_fmt	= &o2xxx_camera_formats[0];
+			xlate->code	= code;
+			xlate++;
+			dev_dbg(dev, "Providing format %s using code %d\n",
+				o2xxx_camera_formats[0].name, code);
+		}
+		break;
+	case V4L2_MBUS_FMT_Y10_1X10:
+		formats++;
+		if (xlate) {
+			xlate->host_fmt	= &o2xxx_camera_formats[1];
+			xlate->code	= code;
+			xlate++;
+			dev_dbg(dev, "Providing format %s using code %d\n",
+				o2xxx_camera_formats[1].name, code);
+		}
+		break;
+	case V4L2_MBUS_FMT_Y8_1X8:
+		formats++;
+		if (xlate) {
+			xlate->host_fmt	= &o2xxx_camera_formats[0];
+			xlate->code	= code;
+			xlate++;
+			dev_dbg(dev, "Providing format %s using code %d\n",
+				o2xxx_camera_formats[0].name, code);
+		}
+		break;
+	default:
+		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++;
+	}
+
+	dev_dbg(icd->parent, "%s() formats %d\n", __func__, formats);
+	return formats;
+}
+
+static int o2xxx_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 o2d_csi_priv *priv = ici->priv;
+	int bytes_per_line;
+	unsigned int height;
+
+	dev_dbg(icd->parent, "%s(), count %d\n", __func__, *count);
+
+	if (fmt) {
+		const struct soc_camera_format_xlate *xlate =
+		soc_camera_xlate_by_fourcc(icd,	fmt->fmt.pix.pixelformat);
+
+		if (!xlate)
+			return -EINVAL;
+		bytes_per_line = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
+							 xlate->host_fmt);
+		height = fmt->fmt.pix.height;
+	} else {
+		/* Called from VIDIOC_REQBUFS or in compatibility mode */
+		bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+		height = icd->user_height;
+	}
+	if (bytes_per_line < 0)
+		return bytes_per_line;
+
+	sizes[0] = bytes_per_line * height;
+
+	alloc_ctxs[0] = priv->alloc_ctx;
+
+	if (!vq->num_buffers)
+		priv->sequence = 0;
+
+	if (!*count)
+		*count = 6;
+
+	/* If *num_planes != 0, we have already verified *count. */
+	if (!*num_planes &&
+	    sizes[0] * *count + priv->buf_total > MAX_VIDEO_MEM * 1024 * 1024)
+		*count = (MAX_VIDEO_MEM * 1024 * 1024 - priv->buf_total) /
+			sizes[0];
+
+	*num_planes = 1;
+
+	return 0;
+}
+
+static void o2xxx_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 o2d_csi_priv *priv = ici->priv;
+	struct o2xxx_camera_buffer *buf = to_o2xxx_vb(vb);
+	unsigned long flags;
+
+	if (vb2_plane_size(vb, 0) < icd->sizeimage) {
+		dev_err(icd->parent, "buffer #%d too small (%lu < %zu)\n",
+		vb->v4l2_buf.index, vb2_plane_size(vb, 0), icd->sizeimage);
+		goto error;
+	}
+
+	if (buf->state == BUF_NEEDS_INIT) {
+		buf->paddr = vb2_dma_contig_plane_dma_addr(vb, 0);
+		buf->bsize = icd->sizeimage;
+		buf->state = BUF_PREPARED;
+	}
+
+	vb2_set_plane_payload(vb, 0, icd->sizeimage);
+
+#ifdef DEBUG
+	if (vb2_plane_vaddr(vb, 0))
+		memset(vb2_plane_vaddr(vb, 0), 0xa5,
+			vb2_get_plane_payload(vb, 0));
+#endif
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	list_add_tail(&buf->queue, &priv->capture);
+
+	if (!priv->active) {
+		priv->active = buf;
+		if (o2xxx_queue_dma_request(priv)) {
+			list_del_init(&buf->queue);
+			priv->active = NULL;
+			spin_unlock_irqrestore(&priv->lock, flags);
+			goto error;
+		}
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return;
+
+error:
+	vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+}
+
+static void o2xxx_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 o2d_csi_priv *priv = ici->priv;
+	struct o2xxx_camera_buffer *buf = to_o2xxx_vb(vb);
+	unsigned long flags;
+
+	dev_dbg(icd->parent,
+		"release%s buf 0x%08lx, queue %sempty\n",
+		priv->active == buf ? " active" : "", (ulong)buf->paddr,
+		list_empty(&buf->queue) ? "" : "not ");
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (priv->active == buf)
+		priv->active = NULL;
+
+	list_del_init(&buf->queue);
+	buf->state = BUF_NEEDS_INIT;
+	priv->buf_total -= vb2_plane_size(vb, 0);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static int o2xxx_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 o2d_csi_priv *priv = ici->priv;
+	struct o2xxx_camera_buffer *buf = to_o2xxx_vb(vb);
+
+	INIT_LIST_HEAD(&buf->queue);
+	buf->state = BUF_NEEDS_INIT;
+	priv->buf_total += vb2_plane_size(vb, 0);
+
+	return 0;
+}
+
+static int o2xxx_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 o2d_csi_priv *priv = ici->priv;
+	struct v4l2_subdev *sd;
+	int ret;
+
+	/* snapshot case */
+	priv->smode = 0;
+
+	sd = soc_camera_to_subdev(priv->icd);
+	ret = v4l2_subdev_call(sd, video, s_stream, priv->smode);
+	if (ret < 0) {
+		dev_err(icd->parent, "Can't switch to snapshot mode\n");
+		return ret;
+	}
+
+	priv->streaming = 1;
+
+	/* now use bus clock for sensor readout */
+	o2xxx_bus_master_clock(priv, 1);
+
+	return 0;
+}
+
+static int o2xxx_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 o2d_csi_priv *priv = ici->priv;
+	struct o2xxx_camera_buffer *buf, *tmp;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	priv->active = NULL;
+	priv->streaming = 0;
+	if (priv->dmareq_queued) {
+		mpc52xx_lpbfifo_abort(&priv->req);
+		priv->dmareq_queued = 0;
+	}
+
+	list_for_each_entry_safe(buf, tmp, &priv->capture, queue) {
+		list_del_init(&buf->queue);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* Enable clock for every instance */
+	o2xxx_bus_master_clock(priv, 0);
+	return 0;
+}
+
+static struct vb2_ops o2xxx_videobuf_ops = {
+	.queue_setup	= o2xxx_videobuf_setup,
+	.buf_init	= o2xxx_videobuf_init,
+	.buf_queue	= o2xxx_videobuf_queue,
+	.buf_cleanup	= o2xxx_videobuf_release,
+	.wait_prepare	= soc_camera_unlock,
+	.wait_finish	= soc_camera_lock,
+	.start_streaming = o2xxx_start_streaming,
+	.stop_streaming	= o2xxx_stop_streaming,
+};
+
+static int o2xxx_camera_init_videobuf(struct vb2_queue *q,
+				      struct soc_camera_device *icd)
+{
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP;
+	q->drv_priv = icd;
+	q->ops = &o2xxx_videobuf_ops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct o2xxx_camera_buffer);
+
+	dev_dbg(icd->parent, "%s()\n", __func__);
+	return vb2_queue_init(q);
+}
+
+static int o2xxx_camera_reqbufs(struct soc_camera_device *icd,
+				struct v4l2_requestbuffers *p)
+{
+	dev_dbg(icd->parent, "%s()\n", __func__);
+	return 0;
+}
+
+static unsigned int o2xxx_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 o2xxx_camera_querycap(struct soc_camera_host *ici,
+				 struct v4l2_capability *cap)
+{
+	strlcpy(cap->card, "o2xxx Camera", sizeof(cap->card));
+	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	return 0;
+}
+
+static int o2xxx_camera_set_bus_param(struct soc_camera_device *icd)
+{
+	return 0;
+}
+
+static struct soc_camera_host_ops o2xxx_soc_camera_host_ops = {
+	.owner		= THIS_MODULE,
+	.add		= o2xxx_camera_add_device,
+	.remove		= o2xxx_camera_remove_device,
+	.set_crop	= o2xxx_camera_set_crop,
+	.set_fmt	= o2xxx_camera_set_fmt,
+	.try_fmt	= o2xxx_camera_try_fmt,
+	.get_formats	= o2xxx_camera_get_formats,
+	.init_videobuf2	= o2xxx_camera_init_videobuf,
+	.reqbufs	= o2xxx_camera_reqbufs,
+	.poll		= o2xxx_camera_poll,
+	.querycap	= o2xxx_camera_querycap,
+	.set_bus_param	= o2xxx_camera_set_bus_param,
+};
+
+static void o2xxx_bus_master_clock(struct o2d_csi_priv *priv, int onoff)
+{
+	pr_debug("%s(): %s\n", __func__, onoff ? "ON" : "OFF");
+	gpio_set_value(priv->imag_master_en, onoff);
+}
+
+static irqreturn_t o2xxx_irq(int irq, void *dev_id)
+{
+	struct o2d_csi_priv *priv = dev_id;
+	static unsigned long cj;
+
+	spin_lock(&priv->lock);
+
+	if (!priv->streaming) {
+		if (printk_timed_ratelimit(&cj, 5000))
+			pr_debug("%s: not streaming\n", __func__);
+		spin_unlock(&priv->lock);
+		return IRQ_HANDLED;
+	}
+
+	if (priv->active && priv->dmareq_queued && !priv->dma_running)
+		o2xxx_start_dma(priv);
+
+	spin_unlock(&priv->lock);
+
+	return IRQ_HANDLED;
+}
+
+static int o2d_csi_sclpc_init(struct platform_device *pdev,
+			      struct o2d_csi_priv *priv)
+{
+	struct device_node *sclpc_np;
+	struct resource sclpc_res;
+
+	sclpc_np = of_find_compatible_node(NULL, NULL, "fsl,mpc5200-lpbfifo");
+	if (!sclpc_np)
+		return -ENODEV;
+
+	if (of_address_to_resource(sclpc_np, 0, &sclpc_res))
+		return -ENODEV;
+
+	priv->lpbfifo_regs_phys = sclpc_res.start;
+	priv->lpbfifo_regs = of_iomap(sclpc_np, 0);
+	of_node_put(sclpc_np);
+	if (!priv->lpbfifo_regs)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static int o2d_csi_lpb_init(struct platform_device *pdev,
+			    struct o2d_csi_priv *priv)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct mpc52xx_mmap_ctl __iomem *immr_regs;
+	struct device_node *immr_np;
+	const unsigned int *prop;
+	void __iomem *csx_start;
+	void __iomem *csx_stop;
+	struct resource res;
+	int chip_select_shift;
+	int csx_cfg_offs;
+	u32 cs_config;
+	u32 val;
+
+	if (of_address_to_resource(np, 0, &res)) {
+		dev_err(&pdev->dev, "coudn't get mapping\n");
+		return -ENODEV;
+	}
+	priv->lpb_cs_phys = res.start;
+
+	if (of_property_read_u32(np, "ifm,csi-addr-bus-width", &val)) {
+		dev_err(&pdev->dev, "missing addr bus width property\n");
+		return -EINVAL;
+	}
+
+	cs_config = 0x00001001;	/* chip enable, read-only */
+
+	switch (val) {
+	case 24:
+		cs_config |= 2 << 10;
+		break;
+	case 16:
+		cs_config |= 1 << 10;
+		break;
+	case 8:
+		/* nop: cs_config |= 0 << 10; */
+		break;
+	default:
+		if (val >= 25)
+			cs_config |= 3 << 10;
+		else {
+			dev_err(&pdev->dev, "invalid addr bus width\n");
+			return -EINVAL;
+		}
+	}
+
+	if (of_property_read_u32(np, "ifm,csi-data-bus-width", &val)) {
+		dev_err(&pdev->dev, "missing data bus width property\n");
+		return -EINVAL;
+	}
+
+	switch (val) {
+	case 32:
+		cs_config |= 3 << 8;
+		break;
+	case 16:
+		cs_config |= 1 << 8;
+		break;
+	case 8:
+		/* nop: cs_config |= 0 << 8;*/
+		break;
+	default:
+		dev_err(&pdev->dev, "invalid data bus width\n");
+		return -EINVAL;
+	}
+
+	if (of_property_read_u32(np, "ifm,csi-wait-cycles", &val)) {
+		dev_err(&pdev->dev, "missing wait cycles property\n");
+		return -EINVAL;
+	}
+
+	cs_config |= (val & 0xff) << 24 | (val & 0xff) << 16;
+
+	if (of_property_read_bool(np, "ifm,csi-byte-swap"))
+		cs_config |= 0x4;
+
+	/* find CS # */
+	prop = of_get_property(np, "reg", &val);
+	if (!prop || val != 12 || !*prop || *prop > 7) {
+		dev_err(&pdev->dev, "invalid reg property\n");
+		return -EINVAL;
+	}
+	priv->chip_select = *prop;
+
+	immr_np = of_find_compatible_node(NULL, NULL, "fsl,mpc5200b-immr");
+	if (!immr_np)
+		return -ENODEV;
+
+	immr_regs = of_iomap(immr_np, 0);
+	of_node_put(immr_np);
+	if (!immr_regs)
+		return -ENODEV;
+
+	switch (priv->chip_select) {
+	case 7:
+		chip_select_shift = 27;
+		csx_cfg_offs = 0x0324;
+		csx_start = &immr_regs->cs7_start;
+		csx_stop = &immr_regs->cs7_stop;
+		break;
+	case 6:
+		chip_select_shift = 26;
+		csx_cfg_offs = 0x0320;
+		csx_start = &immr_regs->cs6_start;
+		csx_stop = &immr_regs->cs6_stop;
+		break;
+	default:
+		chip_select_shift = 16 + priv->chip_select;
+		csx_cfg_offs = 0x0300 + priv->chip_select * 4;
+		csx_start = &immr_regs->cs0_start + priv->chip_select * 2;
+		csx_stop = &immr_regs->cs0_stop + priv->chip_select * 2;
+	}
+
+	dev_dbg(&pdev->dev, "CS%d, cfg 0x%x, shift %d, cfg offs 0x%x\n",
+		priv->chip_select, cs_config, chip_select_shift, csx_cfg_offs);
+
+	/* CS base and size */
+	iowrite32be(priv->lpb_cs_phys  >> 16, csx_start);
+	iowrite32be((priv->lpb_cs_phys + resource_size(&res)) >> 16, csx_stop);
+
+	iowrite32be(cs_config, (void *)immr_regs + csx_cfg_offs);
+
+	/* set CS deadcycles to 0 */
+	val = ioread32be((void *)immr_regs + 0x032c);
+	val &= ~(0xf << (priv->chip_select * 4));
+	iowrite32be(val, (void *)immr_regs + 0x032c);
+
+	/* CS enable */
+	val = ioread32be(&immr_regs->ipbi_ws_ctrl);
+	val |= 0x1 << chip_select_shift;
+	iowrite32be(val, &immr_regs->ipbi_ws_ctrl);
+
+	iounmap(immr_regs);
+	return 0;
+}
+
+static int mpc52xx_xlb_pldis_bsdis(int pldis, int bsdis)
+{
+	struct mpc52xx_xlb __iomem *xlb_regs;
+	struct device_node *xlb_np;
+
+	xlb_np = of_find_compatible_node(NULL, NULL, "fsl,mpc5200-xlb");
+	if (!xlb_np)
+		return -ENODEV;
+	xlb_regs = of_iomap(xlb_np, 0);
+	of_node_put(xlb_np);
+	if (!xlb_regs)
+		return -ENOMEM;
+
+	if (pldis)
+		setbits32(&xlb_regs->config, 0x80000000);
+	else
+		clrbits32(&xlb_regs->config, 0x80000000);
+
+	if (bsdis)
+		setbits32(&xlb_regs->config, 0x00010000);
+	else
+		clrbits32(&xlb_regs->config, 0x00010000);
+
+	iounmap(xlb_regs);
+	return 0;
+}
+
+static int o2d_csi_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct device_node *sdma_np;
+	struct o2d_csi_priv *priv;
+	struct mpc52xx_sdma __iomem *sdma_regs;
+	struct soc_camera_host *soc_host;
+	int ret;
+	int irq;
+
+	irq = irq_of_parse_and_map(np, 0);
+	if (!irq) {
+		dev_err(&pdev->dev, "Coudn't get irq\n");
+		return -ENODEV;
+	}
+
+	if (of_gpio_count(np) < 3) {
+		dev_err(&pdev->dev, "Required gpio cfg. not available\n");
+		return -ENODEV;
+	}
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->irq = irq;
+
+	/* list of video-buffers */
+	INIT_LIST_HEAD(&priv->capture);
+	spin_lock_init(&priv->lock);
+
+	/* disable xlb pipelining and bestcomm xlb snooping */
+	ret = mpc52xx_xlb_pldis_bsdis(1, 1);
+	if (ret) {
+		dev_err(&pdev->dev, "Can't configure xlb\n");
+		return ret;
+	}
+
+	sdma_np = of_find_compatible_node(NULL, NULL, "fsl,mpc5200-bestcomm");
+	if (!sdma_np)
+		return -ENODEV;
+	sdma_regs = of_iomap(sdma_np, 0);
+	of_node_put(sdma_np);
+	if (!sdma_regs)
+		return -ENOMEM;
+
+	/* disable prefetch on commbus */
+	setbits16(&sdma_regs->PtdCntrl, 1);
+	iounmap(sdma_regs);
+
+	ret = o2d_csi_sclpc_init(pdev, priv);
+	if (ret)
+		goto err_request3;
+
+	ret = o2d_csi_lpb_init(pdev, priv);
+	if (ret)
+		goto err_request3;
+
+	priv->imag_capture = of_get_gpio(np, 0);
+	priv->imag_reset = of_get_gpio(np, 1);
+	priv->imag_master_en = of_get_gpio(np, 2);
+
+	if (!gpio_is_valid(priv->imag_capture) ||
+	    !gpio_is_valid(priv->imag_reset) ||
+	    !gpio_is_valid(priv->imag_master_en)) {
+		dev_err(&pdev->dev, "%s: invalid GPIO pins\n", np->full_name);
+		return -ENODEV;
+	}
+
+	priv->gpt_np = of_parse_phandle(np, "ifm,csi-clk-handle", 0);
+	if (!priv->gpt_np)
+		return -ENODEV;
+
+	priv->gpt = of_iomap(priv->gpt_np, 0);
+	if (!priv->gpt) {
+		ret = -ENOMEM;
+		goto err_iomap;
+	}
+
+	ret = gpio_request(priv->imag_capture, "imag_capture");
+	if (ret)
+		goto err_request1;
+	ret = gpio_request(priv->imag_reset, "imag_reset");
+	if (ret)
+		goto err_request2;
+	ret = gpio_request(priv->imag_master_en, "imag_master_en");
+	if (ret)
+		goto err_request3;
+
+	o2xxx_set_sysclk(priv, 6, 1);
+
+	gpio_direction_output(priv->imag_capture, 0);
+	gpio_direction_output(priv->imag_reset, 1);
+	gpio_direction_output(priv->imag_master_en, 0);
+	o2xxx_bus_master_clock(priv, 0);
+
+	o2xxx_sensor_reset(priv);
+	mdelay(20);
+
+	priv->image_length = DEFAULT_IMAGE_LENGTH;
+
+	priv->soc_cam_pdev = platform_device_register_resndata(NULL,
+				"soc-camera-pdrv", 0, NULL, 0,
+				&iclink_mt9v022, sizeof(iclink_mt9v022));
+
+	soc_host		= &priv->soc_host;
+	soc_host->drv_name	= O2XXX_CAM_DRV_NAME;
+	soc_host->ops		= &o2xxx_soc_camera_host_ops;
+	soc_host->priv          = priv;
+	soc_host->v4l2_dev.dev  = &pdev->dev;
+	soc_host->nr            = 0;
+
+	priv->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(priv->alloc_ctx)) {
+		ret = PTR_ERR(priv->alloc_ctx);
+		goto err_request3;
+	}
+
+	ret = soc_camera_host_register(soc_host);
+	if (ret) {
+		dev_err(&pdev->dev, "Coudn't register host\n");
+		goto err_hostreg;
+	}
+
+	o2xxx_set_sysclk(priv, 6, 1);
+	mdelay(20);
+
+	ret = request_irq(irq, o2xxx_irq, 0, "mpc52xx-csi", priv);
+	if (ret)
+		goto err_hostreg;
+
+	dev_info(&pdev->dev, "initialised\n");
+
+	return 0;
+
+err_hostreg:
+	vb2_dma_contig_cleanup_ctx(priv->alloc_ctx);
+err_request3:
+	gpio_free(priv->imag_reset);
+err_request2:
+	gpio_free(priv->imag_capture);
+err_request1:
+	iounmap(priv->gpt);
+err_iomap:
+	if (priv->gpt_np)
+		of_node_put(priv->gpt_np);
+	return ret;
+}
+
+static int o2d_csi_remove(struct platform_device *pdev)
+{
+	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
+	struct o2d_csi_priv *priv = container_of(soc_host, struct o2d_csi_priv,
+						 soc_host);
+
+	dev_info(&pdev->dev, "released\n");
+
+	/* stop sensor clock */
+	o2xxx_set_sysclk(priv, 6, 0);
+
+	free_irq(priv->irq, priv);
+
+	soc_camera_host_unregister(soc_host);
+
+	vb2_dma_contig_cleanup_ctx(priv->alloc_ctx);
+
+	if (priv->gpt_np)
+		of_node_put(priv->gpt_np);
+
+	iounmap(priv->lpbfifo_regs);
+	iounmap(priv->gpt);
+
+	platform_device_unregister(priv->soc_cam_pdev);
+
+	/* hold sensor reset */
+	gpio_set_value(priv->imag_reset, 0);
+	gpio_free(priv->imag_master_en);
+	gpio_free(priv->imag_reset);
+	gpio_free(priv->imag_capture);
+
+	/* xlb pipelining and snooping on */
+	mpc52xx_xlb_pldis_bsdis(0, 0);
+	return 0;
+}
+
+static struct of_device_id __devinitdata o2d_csi_match[] = {
+	{ .compatible = "ifm,o2d-csi", },
+	{ /* end */ }
+};
+MODULE_DEVICE_TABLE(of, o2d_csi_match);
+
+static struct platform_driver o2d_csi_driver = {
+	.driver = {
+		.name = "o2d_csi",
+		.owner = THIS_MODULE,
+		.of_match_table = o2d_csi_match,
+	},
+	.probe		= o2d_csi_probe,
+	.remove		= __devexit_p(o2d_csi_remove),
+};
+
+module_platform_driver(o2d_csi_driver);
+
+MODULE_AUTHOR("Anatolij Gustschin, agust@denx.de");
+MODULE_DESCRIPTION("o2d camera sensor interface driver");
+MODULE_LICENSE("GPL");
