Merge branch 'master' into for-next

Fast-forwarded to current state of Linus' tree as there are patches to be
applied for files that didn't exist on the old branch.

5 files changed, 1107 insertions, 930 deletions

diff --git a/drivers/media/video/s5p-fimc/fimc-capture.c b/drivers/media/video/s5p-fimc/fimc-capture.c
index 2f500809f53d..95f8b4e11e46 100644
--- a/drivers/media/video/s5p-fimc/fimc-capture.c
+++ b/drivers/media/video/s5p-fimc/fimc-capture.c
@@ -27,13 +27,13 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-mem2mem.h>
-#include <media/videobuf-core.h>
-#include <media/videobuf-dma-contig.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
 
 #include "fimc-core.h"
 
 static struct v4l2_subdev *fimc_subdev_register(struct fimc_dev *fimc,
-					    struct s3c_fimc_isp_info *isp_info)
+					    struct s5p_fimc_isp_info *isp_info)
 {
 	struct i2c_adapter *i2c_adap;
 	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
@@ -86,19 +86,19 @@ static void fimc_subdev_unregister(struct fimc_dev *fimc)
 static int fimc_subdev_attach(struct fimc_dev *fimc, int index)
 {
 	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
-	struct s3c_platform_fimc *pdata = fimc->pdata;
-	struct s3c_fimc_isp_info *isp_info;
+	struct s5p_platform_fimc *pdata = fimc->pdata;
+	struct s5p_fimc_isp_info *isp_info;
 	struct v4l2_subdev *sd;
 	int i;
 
-	for (i = 0; i < FIMC_MAX_CAMIF_CLIENTS; ++i) {
-		isp_info = pdata->isp_info[i];
+	for (i = 0; i < pdata->num_clients; ++i) {
+		isp_info = &pdata->isp_info[i];
 
-		if (!isp_info || (index >= 0 && i != index))
+		if (index >= 0 && i != index)
 			continue;
 
 		sd = fimc_subdev_register(fimc, isp_info);
-		if (sd) {
+		if (!IS_ERR_OR_NULL(sd)) {
 			vid_cap->sd = sd;
 			vid_cap->input_index = i;
 
@@ -113,60 +113,42 @@ static int fimc_subdev_attach(struct fimc_dev *fimc, int index)
 	return -ENODEV;
 }
 
-static int fimc_isp_subdev_init(struct fimc_dev *fimc, int index)
+static int fimc_isp_subdev_init(struct fimc_dev *fimc, unsigned int index)
 {
-	struct s3c_fimc_isp_info *isp_info;
+	struct s5p_fimc_isp_info *isp_info;
+	struct s5p_platform_fimc *pdata = fimc->pdata;
 	int ret;
 
-	ret = fimc_subdev_attach(fimc, index);
-	if (ret)
-		return ret;
+	if (index >= pdata->num_clients)
+		return -EINVAL;
 
-	isp_info = fimc->pdata->isp_info[fimc->vid_cap.input_index];
-	ret = fimc_hw_set_camera_polarity(fimc, isp_info);
-	if (!ret) {
-		ret = v4l2_subdev_call(fimc->vid_cap.sd, core,
-				       s_power, 1);
-		if (!ret)
-			return ret;
-	}
+	isp_info = &pdata->isp_info[index];
 
-	fimc_subdev_unregister(fimc);
-	err("ISP initialization failed: %d", ret);
-	return ret;
-}
+	if (isp_info->clk_frequency)
+		clk_set_rate(fimc->clock[CLK_CAM], isp_info->clk_frequency);
 
-/*
- * At least one buffer on the pending_buf_q queue is required.
- * Locking: The caller holds fimc->slock spinlock.
- */
-int fimc_vid_cap_buf_queue(struct fimc_dev *fimc,
-			     struct fimc_vid_buffer *fimc_vb)
-{
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
-	struct fimc_ctx *ctx = cap->ctx;
-	int ret = 0;
+	ret = clk_enable(fimc->clock[CLK_CAM]);
+	if (ret)
+		return ret;
 
-	BUG_ON(!fimc || !fimc_vb);
+	ret = fimc_subdev_attach(fimc, index);
+	if (ret)
+		return ret;
 
-	ret = fimc_prepare_addr(ctx, fimc_vb, &ctx->d_frame,
-				&fimc_vb->paddr);
+	ret = fimc_hw_set_camera_polarity(fimc, isp_info);
 	if (ret)
 		return ret;
 
-	if (test_bit(ST_CAPT_STREAM, &fimc->state)) {
-		fimc_pending_queue_add(cap, fimc_vb);
-	} else {
-		/* Setup the buffer directly for processing. */
-		int buf_id = (cap->reqbufs_count == 1) ? -1 : cap->buf_index;
-		fimc_hw_set_output_addr(fimc, &fimc_vb->paddr, buf_id);
+	ret = v4l2_subdev_call(fimc->vid_cap.sd, core, s_power, 1);
+	if (!ret)
+		return ret;
+
+	/* enabling power failed so unregister subdev */
+	fimc_subdev_unregister(fimc);
 
-		fimc_vb->index = cap->buf_index;
-		active_queue_add(cap, fimc_vb);
+	v4l2_err(&fimc->vid_cap.v4l2_dev, "ISP initialization failed: %d\n",
+		 ret);
 
-		if (++cap->buf_index >= FIMC_MAX_OUT_BUFS)
-			cap->buf_index = 0;
-	}
 	return ret;
 }
 
@@ -174,7 +156,7 @@ static int fimc_stop_capture(struct fimc_dev *fimc)
 {
 	unsigned long flags;
 	struct fimc_vid_cap *cap;
-	int ret;
+	struct fimc_vid_buffer *buf;
 
 	cap = &fimc->vid_cap;
 
@@ -187,24 +169,224 @@ static int fimc_stop_capture(struct fimc_dev *fimc)
 	spin_unlock_irqrestore(&fimc->slock, flags);
 
 	wait_event_timeout(fimc->irq_queue,
-			   test_bit(ST_CAPT_SHUT, &fimc->state),
+			   !test_bit(ST_CAPT_SHUT, &fimc->state),
 			   FIMC_SHUTDOWN_TIMEOUT);
 
-	ret = v4l2_subdev_call(cap->sd, video, s_stream, 0);
-	if (ret)
-		v4l2_err(&fimc->vid_cap.v4l2_dev, "s_stream(0) failed\n");
+	v4l2_subdev_call(cap->sd, video, s_stream, 0);
 
 	spin_lock_irqsave(&fimc->slock, flags);
 	fimc->state &= ~(1 << ST_CAPT_RUN | 1 << ST_CAPT_PEND |
-			1 << ST_CAPT_STREAM);
+			 1 << ST_CAPT_SHUT | 1 << ST_CAPT_STREAM);
 
 	fimc->vid_cap.active_buf_cnt = 0;
+
+	/* Release buffers that were enqueued in the driver by videobuf2. */
+	while (!list_empty(&cap->pending_buf_q)) {
+		buf = pending_queue_pop(cap);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+	}
+
+	while (!list_empty(&cap->active_buf_q)) {
+		buf = active_queue_pop(cap);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+	}
+
 	spin_unlock_irqrestore(&fimc->slock, flags);
 
 	dbg("state: 0x%lx", fimc->state);
 	return 0;
 }
 
+static int start_streaming(struct vb2_queue *q)
+{
+	struct fimc_ctx *ctx = q->drv_priv;
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct s5p_fimc_isp_info *isp_info;
+	int ret;
+
+	fimc_hw_reset(fimc);
+
+	ret = v4l2_subdev_call(fimc->vid_cap.sd, video, s_stream, 1);
+	if (ret && ret != -ENOIOCTLCMD)
+		return ret;
+
+	ret = fimc_prepare_config(ctx, ctx->state);
+	if (ret)
+		return ret;
+
+	isp_info = &fimc->pdata->isp_info[fimc->vid_cap.input_index];
+	fimc_hw_set_camera_type(fimc, isp_info);
+	fimc_hw_set_camera_source(fimc, isp_info);
+	fimc_hw_set_camera_offset(fimc, &ctx->s_frame);
+
+	if (ctx->state & FIMC_PARAMS) {
+		ret = fimc_set_scaler_info(ctx);
+		if (ret) {
+			err("Scaler setup error");
+			return ret;
+		}
+		fimc_hw_set_input_path(ctx);
+		fimc_hw_set_prescaler(ctx);
+		fimc_hw_set_mainscaler(ctx);
+		fimc_hw_set_target_format(ctx);
+		fimc_hw_set_rotation(ctx);
+		fimc_hw_set_effect(ctx);
+	}
+
+	fimc_hw_set_output_path(ctx);
+	fimc_hw_set_out_dma(ctx);
+
+	INIT_LIST_HEAD(&fimc->vid_cap.pending_buf_q);
+	INIT_LIST_HEAD(&fimc->vid_cap.active_buf_q);
+	fimc->vid_cap.active_buf_cnt = 0;
+	fimc->vid_cap.frame_count = 0;
+	fimc->vid_cap.buf_index = 0;
+
+	set_bit(ST_CAPT_PEND, &fimc->state);
+
+	return 0;
+}
+
+static int stop_streaming(struct vb2_queue *q)
+{
+	struct fimc_ctx *ctx = q->drv_priv;
+	struct fimc_dev *fimc = ctx->fimc_dev;
+
+	if (!fimc_capture_active(fimc))
+		return -EINVAL;
+
+	return fimc_stop_capture(fimc);
+}
+
+static unsigned int get_plane_size(struct fimc_frame *fr, unsigned int plane)
+{
+	if (!fr || plane >= fr->fmt->memplanes)
+		return 0;
+
+	dbg("%s: w: %d. h: %d. depth[%d]: %d",
+	    __func__, fr->width, fr->height, plane, fr->fmt->depth[plane]);
+
+	return fr->f_width * fr->f_height * fr->fmt->depth[plane] / 8;
+
+}
+
+static int queue_setup(struct vb2_queue *vq, unsigned int *num_buffers,
+		       unsigned int *num_planes, unsigned long sizes[],
+		       void *allocators[])
+{
+	struct fimc_ctx *ctx = vq->drv_priv;
+	struct fimc_fmt *fmt = ctx->d_frame.fmt;
+	int i;
+
+	if (!fmt)
+		return -EINVAL;
+
+	*num_planes = fmt->memplanes;
+
+	dbg("%s, buffer count=%d, plane count=%d",
+	    __func__, *num_buffers, *num_planes);
+
+	for (i = 0; i < fmt->memplanes; i++) {
+		sizes[i] = get_plane_size(&ctx->d_frame, i);
+		dbg("plane: %u, plane_size: %lu", i, sizes[i]);
+		allocators[i] = ctx->fimc_dev->alloc_ctx;
+	}
+
+	return 0;
+}
+
+static int buffer_init(struct vb2_buffer *vb)
+{
+	/* TODO: */
+	return 0;
+}
+
+static int buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct fimc_ctx *ctx = vq->drv_priv;
+	struct v4l2_device *v4l2_dev = &ctx->fimc_dev->m2m.v4l2_dev;
+	int i;
+
+	if (!ctx->d_frame.fmt || vq->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
+
+	for (i = 0; i < ctx->d_frame.fmt->memplanes; i++) {
+		unsigned long size = get_plane_size(&ctx->d_frame, i);
+
+		if (vb2_plane_size(vb, i) < size) {
+			v4l2_err(v4l2_dev, "User buffer too small (%ld < %ld)\n",
+				 vb2_plane_size(vb, i), size);
+			return -EINVAL;
+		}
+
+		vb2_set_plane_payload(vb, i, size);
+	}
+
+	return 0;
+}
+
+static void buffer_queue(struct vb2_buffer *vb)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct fimc_vid_buffer *buf
+		= container_of(vb, struct fimc_vid_buffer, vb);
+	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
+	unsigned long flags;
+	int min_bufs;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	fimc_prepare_addr(ctx, &buf->vb, &ctx->d_frame, &buf->paddr);
+
+	if (!test_bit(ST_CAPT_STREAM, &fimc->state)
+	     && vid_cap->active_buf_cnt < FIMC_MAX_OUT_BUFS) {
+		/* Setup the buffer directly for processing. */
+		int buf_id = (vid_cap->reqbufs_count == 1) ? -1 :
+				vid_cap->buf_index;
+
+		fimc_hw_set_output_addr(fimc, &buf->paddr, buf_id);
+		buf->index = vid_cap->buf_index;
+		active_queue_add(vid_cap, buf);
+
+		if (++vid_cap->buf_index >= FIMC_MAX_OUT_BUFS)
+			vid_cap->buf_index = 0;
+	} else {
+		fimc_pending_queue_add(vid_cap, buf);
+	}
+
+	min_bufs = vid_cap->reqbufs_count > 1 ? 2 : 1;
+
+	if (vid_cap->active_buf_cnt >= min_bufs &&
+	    !test_and_set_bit(ST_CAPT_STREAM, &fimc->state))
+		fimc_activate_capture(ctx);
+
+	spin_unlock_irqrestore(&fimc->slock, flags);
+}
+
+static void fimc_lock(struct vb2_queue *vq)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
+	mutex_lock(&ctx->fimc_dev->lock);
+}
+
+static void fimc_unlock(struct vb2_queue *vq)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
+	mutex_unlock(&ctx->fimc_dev->lock);
+}
+
+static struct vb2_ops fimc_capture_qops = {
+	.queue_setup		= queue_setup,
+	.buf_prepare		= buffer_prepare,
+	.buf_queue		= buffer_queue,
+	.buf_init		= buffer_init,
+	.wait_prepare		= fimc_unlock,
+	.wait_finish		= fimc_lock,
+	.start_streaming	= start_streaming,
+	.stop_streaming		= stop_streaming,
+};
+
 static int fimc_capture_open(struct file *file)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
@@ -216,44 +398,36 @@ static int fimc_capture_open(struct file *file)
 	if (fimc_m2m_active(fimc))
 		return -EBUSY;
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
 	if (++fimc->vid_cap.refcnt == 1) {
-		ret = fimc_isp_subdev_init(fimc, -1);
+		ret = fimc_isp_subdev_init(fimc, 0);
 		if (ret) {
 			fimc->vid_cap.refcnt--;
-			ret = -EIO;
+			return -EIO;
 		}
 	}
 
 	file->private_data = fimc->vid_cap.ctx;
 
-	mutex_unlock(&fimc->lock);
-	return ret;
+	return 0;
 }
 
 static int fimc_capture_close(struct file *file)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
 	dbg("pid: %d, state: 0x%lx", task_pid_nr(current), fimc->state);
 
 	if (--fimc->vid_cap.refcnt == 0) {
 		fimc_stop_capture(fimc);
-
-		videobuf_stop(&fimc->vid_cap.vbq);
-		videobuf_mmap_free(&fimc->vid_cap.vbq);
+		vb2_queue_release(&fimc->vid_cap.vbq);
 
 		v4l2_err(&fimc->vid_cap.v4l2_dev, "releasing ISP\n");
+
 		v4l2_subdev_call(fimc->vid_cap.sd, core, s_power, 0);
+		clk_disable(fimc->clock[CLK_CAM]);
 		fimc_subdev_unregister(fimc);
 	}
 
-	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
@@ -262,32 +436,16 @@ static unsigned int fimc_capture_poll(struct file *file,
 {
 	struct fimc_ctx *ctx = file->private_data;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
-	int ret;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return POLLERR;
 
-	ret = videobuf_poll_stream(file, &cap->vbq, wait);
-	mutex_unlock(&fimc->lock);
-
-	return ret;
+	return vb2_poll(&fimc->vid_cap.vbq, file, wait);
 }
 
 static int fimc_capture_mmap(struct file *file, struct vm_area_struct *vma)
 {
 	struct fimc_ctx *ctx = file->private_data;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
-	int ret;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
 
-	ret = videobuf_mmap_mapper(&cap->vbq, vma);
-	mutex_unlock(&fimc->lock);
-
-	return ret;
+	return vb2_mmap(&fimc->vid_cap.vbq, vma);
 }
 
 /* video device file operations */
@@ -310,7 +468,8 @@ static int fimc_vidioc_querycap_capture(struct file *file, void *priv,
 	strncpy(cap->card, fimc->pdev->name, sizeof(cap->card) - 1);
 	cap->bus_info[0] = 0;
 	cap->version = KERNEL_VERSION(1, 0, 0);
-	cap->capabilities = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE;
+	cap->capabilities = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE |
+			    V4L2_CAP_VIDEO_CAPTURE_MPLANE;
 
 	return 0;
 }
@@ -351,57 +510,52 @@ static int sync_capture_fmt(struct fimc_ctx *ctx)
 	return 0;
 }
 
-static int fimc_cap_s_fmt(struct file *file, void *priv,
-			     struct v4l2_format *f)
+static int fimc_cap_s_fmt_mplane(struct file *file, void *priv,
+				 struct v4l2_format *f)
 {
 	struct fimc_ctx *ctx = priv;
 	struct fimc_dev *fimc = ctx->fimc_dev;
 	struct fimc_frame *frame;
-	struct v4l2_pix_format *pix;
+	struct v4l2_pix_format_mplane *pix;
 	int ret;
+	int i;
 
-	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 		return -EINVAL;
 
-	ret = fimc_vidioc_try_fmt(file, priv, f);
+	ret = fimc_vidioc_try_fmt_mplane(file, priv, f);
 	if (ret)
 		return ret;
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
-	if (fimc_capture_active(fimc)) {
-		ret = -EBUSY;
-		goto sf_unlock;
-	}
+	if (vb2_is_streaming(&fimc->vid_cap.vbq) || fimc_capture_active(fimc))
+		return -EBUSY;
 
 	frame = &ctx->d_frame;
 
-	pix = &f->fmt.pix;
+	pix = &f->fmt.pix_mp;
 	frame->fmt = find_format(f, FMT_FLAGS_M2M | FMT_FLAGS_CAM);
 	if (!frame->fmt) {
 		err("fimc target format not found\n");
-		ret = -EINVAL;
-		goto sf_unlock;
+		return -EINVAL;
 	}
 
+	for (i = 0; i < frame->fmt->colplanes; i++)
+		frame->payload[i] = pix->plane_fmt[i].bytesperline * pix->height;
+
 	/* Output DMA frame pixel size and offsets. */
-	frame->f_width	= pix->bytesperline * 8 / frame->fmt->depth;
+	frame->f_width = pix->plane_fmt[0].bytesperline * 8
+			/ frame->fmt->depth[0];
 	frame->f_height = pix->height;
 	frame->width	= pix->width;
 	frame->height	= pix->height;
 	frame->o_width	= pix->width;
 	frame->o_height = pix->height;
-	frame->size	= (pix->width * pix->height * frame->fmt->depth) >> 3;
 	frame->offs_h	= 0;
 	frame->offs_v	= 0;
 
-	ret = sync_capture_fmt(ctx);
-
 	ctx->state |= (FIMC_PARAMS | FIMC_DST_FMT);
 
-sf_unlock:
-	mutex_unlock(&fimc->lock);
+	ret = sync_capture_fmt(ctx);
 	return ret;
 }
 
@@ -409,15 +563,13 @@ static int fimc_cap_enum_input(struct file *file, void *priv,
 				     struct v4l2_input *i)
 {
 	struct fimc_ctx *ctx = priv;
-	struct s3c_platform_fimc *pldata = ctx->fimc_dev->pdata;
-	struct s3c_fimc_isp_info *isp_info;
+	struct s5p_platform_fimc *pldata = ctx->fimc_dev->pdata;
+	struct s5p_fimc_isp_info *isp_info;
 
-	if (i->index >= FIMC_MAX_CAMIF_CLIENTS)
+	if (i->index >= pldata->num_clients)
 		return -EINVAL;
 
-	isp_info = pldata->isp_info[i->index];
-	if (isp_info == NULL)
-		return -EINVAL;
+	isp_info = &pldata->isp_info[i->index];
 
 	i->type = V4L2_INPUT_TYPE_CAMERA;
 	strncpy(i->name, isp_info->board_info->type, 32);
@@ -429,34 +581,27 @@ static int fimc_cap_s_input(struct file *file, void *priv,
 {
 	struct fimc_ctx *ctx = priv;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct s3c_platform_fimc *pdata = fimc->pdata;
-	int ret;
+	struct s5p_platform_fimc *pdata = fimc->pdata;
 
 	if (fimc_capture_active(ctx->fimc_dev))
 		return -EBUSY;
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
+	if (i >= pdata->num_clients)
+		return -EINVAL;
 
-	if (i >= FIMC_MAX_CAMIF_CLIENTS || !pdata->isp_info[i]) {
-		ret = -EINVAL;
-		goto si_unlock;
-	}
 
 	if (fimc->vid_cap.sd) {
-		ret = v4l2_subdev_call(fimc->vid_cap.sd, core, s_power, 0);
+		int ret = v4l2_subdev_call(fimc->vid_cap.sd, core, s_power, 0);
 		if (ret)
 			err("s_power failed: %d", ret);
+
+		clk_disable(fimc->clock[CLK_CAM]);
 	}
 
 	/* Release the attached sensor subdevice. */
 	fimc_subdev_unregister(fimc);
 
-	ret = fimc_isp_subdev_init(fimc, i);
-
-si_unlock:
-	mutex_unlock(&fimc->lock);
-	return ret;
+	return fimc_isp_subdev_init(fimc, i);
 }
 
 static int fimc_cap_g_input(struct file *file, void *priv,
@@ -470,66 +615,20 @@ static int fimc_cap_g_input(struct file *file, void *priv,
 }
 
 static int fimc_cap_streamon(struct file *file, void *priv,
-			   enum v4l2_buf_type type)
+			     enum v4l2_buf_type type)
 {
-	struct s3c_fimc_isp_info *isp_info;
 	struct fimc_ctx *ctx = priv;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	int ret = -EBUSY;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
 
 	if (fimc_capture_active(fimc) || !fimc->vid_cap.sd)
-		goto s_unlock;
+		return -EBUSY;
 
 	if (!(ctx->state & FIMC_DST_FMT)) {
 		v4l2_err(&fimc->vid_cap.v4l2_dev, "Format is not set\n");
-		ret = -EINVAL;
-		goto s_unlock;
-	}
-
-	ret = v4l2_subdev_call(fimc->vid_cap.sd, video, s_stream, 1);
-	if (ret && ret != -ENOIOCTLCMD)
-		goto s_unlock;
-
-	ret = fimc_prepare_config(ctx, ctx->state);
-	if (ret)
-		goto s_unlock;
-
-	isp_info = fimc->pdata->isp_info[fimc->vid_cap.input_index];
-	fimc_hw_set_camera_type(fimc, isp_info);
-	fimc_hw_set_camera_source(fimc, isp_info);
-	fimc_hw_set_camera_offset(fimc, &ctx->s_frame);
-
-	if (ctx->state & FIMC_PARAMS) {
-		ret = fimc_set_scaler_info(ctx);
-		if (ret) {
-			err("Scaler setup error");
-			goto s_unlock;
-		}
-		fimc_hw_set_input_path(ctx);
-		fimc_hw_set_scaler(ctx);
-		fimc_hw_set_target_format(ctx);
-		fimc_hw_set_rotation(ctx);
-		fimc_hw_set_effect(ctx);
+		return -EINVAL;
 	}
 
-	fimc_hw_set_output_path(ctx);
-	fimc_hw_set_out_dma(ctx);
-
-	INIT_LIST_HEAD(&fimc->vid_cap.pending_buf_q);
-	INIT_LIST_HEAD(&fimc->vid_cap.active_buf_q);
-	fimc->vid_cap.active_buf_cnt = 0;
-	fimc->vid_cap.frame_count = 0;
-	fimc->vid_cap.buf_index = fimc_hw_get_frame_index(fimc);
-
-	set_bit(ST_CAPT_PEND, &fimc->state);
-	ret = videobuf_streamon(&fimc->vid_cap.vbq);
-
-s_unlock:
-	mutex_unlock(&fimc->lock);
-	return ret;
+	return vb2_streamon(&fimc->vid_cap.vbq, type);
 }
 
 static int fimc_cap_streamoff(struct file *file, void *priv,
@@ -537,46 +636,22 @@ static int fimc_cap_streamoff(struct file *file, void *priv,
 {
 	struct fimc_ctx *ctx = priv;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
-	unsigned long flags;
-	int ret;
-
-	spin_lock_irqsave(&fimc->slock, flags);
-	if (!fimc_capture_running(fimc) && !fimc_capture_pending(fimc)) {
-		spin_unlock_irqrestore(&fimc->slock, flags);
-		dbg("state: 0x%lx", fimc->state);
-		return -EINVAL;
-	}
-	spin_unlock_irqrestore(&fimc->slock, flags);
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
-	fimc_stop_capture(fimc);
-	ret = videobuf_streamoff(&cap->vbq);
-	mutex_unlock(&fimc->lock);
-	return ret;
+	return vb2_streamoff(&fimc->vid_cap.vbq, type);
 }
 
 static int fimc_cap_reqbufs(struct file *file, void *priv,
-			  struct v4l2_requestbuffers *reqbufs)
+			    struct v4l2_requestbuffers *reqbufs)
 {
 	struct fimc_ctx *ctx = priv;
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
+	struct fimc_vid_cap *cap = &ctx->fimc_dev->vid_cap;
 	int ret;
 
-	if (fimc_capture_active(ctx->fimc_dev))
-		return -EBUSY;
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
-	ret = videobuf_reqbufs(&cap->vbq, reqbufs);
+	ret = vb2_reqbufs(&cap->vbq, reqbufs);
 	if (!ret)
 		cap->reqbufs_count = reqbufs->count;
 
-	mutex_unlock(&fimc->lock);
 	return ret;
 }
 
@@ -586,43 +661,23 @@ static int fimc_cap_querybuf(struct file *file, void *priv,
 	struct fimc_ctx *ctx = priv;
 	struct fimc_vid_cap *cap = &ctx->fimc_dev->vid_cap;
 
-	if (fimc_capture_active(ctx->fimc_dev))
-		return -EBUSY;
-
-	return videobuf_querybuf(&cap->vbq, buf);
+	return vb2_querybuf(&cap->vbq, buf);
 }
 
 static int fimc_cap_qbuf(struct file *file, void *priv,
 			  struct v4l2_buffer *buf)
 {
 	struct fimc_ctx *ctx = priv;
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
-	int ret;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
-	ret = videobuf_qbuf(&cap->vbq, buf);
-
-	mutex_unlock(&fimc->lock);
-	return ret;
+	struct fimc_vid_cap *cap = &ctx->fimc_dev->vid_cap;
+	return vb2_qbuf(&cap->vbq, buf);
 }
 
 static int fimc_cap_dqbuf(struct file *file, void *priv,
 			   struct v4l2_buffer *buf)
 {
 	struct fimc_ctx *ctx = priv;
-	int ret;
-
-	if (mutex_lock_interruptible(&ctx->fimc_dev->lock))
-		return -ERESTARTSYS;
-
-	ret = videobuf_dqbuf(&ctx->fimc_dev->vid_cap.vbq, buf,
+	return vb2_dqbuf(&ctx->fimc_dev->vid_cap.vbq, buf,
 		file->f_flags & O_NONBLOCK);
-
-	mutex_unlock(&ctx->fimc_dev->lock);
-	return ret;
 }
 
 static int fimc_cap_s_ctrl(struct file *file, void *priv,
@@ -631,9 +686,6 @@ static int fimc_cap_s_ctrl(struct file *file, void *priv,
 	struct fimc_ctx *ctx = priv;
 	int ret = -EINVAL;
 
-	if (mutex_lock_interruptible(&ctx->fimc_dev->lock))
-		return -ERESTARTSYS;
-
 	/* Allow any controls but 90/270 rotation while streaming */
 	if (!fimc_capture_active(ctx->fimc_dev) ||
 	    ctrl->id != V4L2_CID_ROTATE ||
@@ -648,8 +700,6 @@ static int fimc_cap_s_ctrl(struct file *file, void *priv,
 	if (ret == -EINVAL)
 		ret = v4l2_subdev_call(ctx->fimc_dev->vid_cap.sd,
 				       core, s_ctrl, ctrl);
-
-	mutex_unlock(&ctx->fimc_dev->lock);
 	return ret;
 }
 
@@ -658,22 +708,18 @@ static int fimc_cap_cropcap(struct file *file, void *fh,
 {
 	struct fimc_frame *f;
 	struct fimc_ctx *ctx = fh;
-	struct fimc_dev *fimc = ctx->fimc_dev;
 
-	if (cr->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (cr->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 		return -EINVAL;
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
 	f = &ctx->s_frame;
+
 	cr->bounds.left		= 0;
 	cr->bounds.top		= 0;
 	cr->bounds.width	= f->o_width;
 	cr->bounds.height	= f->o_height;
 	cr->defrect		= cr->bounds;
 
-	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
@@ -681,19 +727,14 @@ static int fimc_cap_g_crop(struct file *file, void *fh, struct v4l2_crop *cr)
 {
 	struct fimc_frame *f;
 	struct fimc_ctx *ctx = file->private_data;
-	struct fimc_dev *fimc = ctx->fimc_dev;
-
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
 
 	f = &ctx->s_frame;
+
 	cr->c.left	= f->offs_h;
 	cr->c.top	= f->offs_v;
 	cr->c.width	= f->width;
 	cr->c.height	= f->height;
 
-	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
@@ -712,41 +753,38 @@ static int fimc_cap_s_crop(struct file *file, void *fh,
 	if (ret)
 		return ret;
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
 	if (!(ctx->state & FIMC_DST_FMT)) {
 		v4l2_err(&fimc->vid_cap.v4l2_dev,
 			 "Capture color format not set\n");
-		goto sc_unlock;
+		return -EINVAL; /* TODO: make sure this is the right value */
 	}
 
 	f = &ctx->s_frame;
 	/* Check for the pixel scaling ratio when cropping input image. */
-	ret = fimc_check_scaler_ratio(&cr->c, &ctx->d_frame);
+	ret = fimc_check_scaler_ratio(cr->c.width, cr->c.height,
+				      ctx->d_frame.width, ctx->d_frame.height,
+				      ctx->rotation);
 	if (ret) {
-		v4l2_err(&fimc->vid_cap.v4l2_dev, "Out of the scaler range");
-	} else {
-		ret = 0;
-		f->offs_h = cr->c.left;
-		f->offs_v = cr->c.top;
-		f->width  = cr->c.width;
-		f->height = cr->c.height;
+		v4l2_err(&fimc->vid_cap.v4l2_dev, "Out of the scaler range\n");
+		return ret;
 	}
 
-sc_unlock:
-	mutex_unlock(&fimc->lock);
-	return ret;
+	f->offs_h = cr->c.left;
+	f->offs_v = cr->c.top;
+	f->width  = cr->c.width;
+	f->height = cr->c.height;
+
+	return 0;
 }
 
 
 static const struct v4l2_ioctl_ops fimc_capture_ioctl_ops = {
 	.vidioc_querycap		= fimc_vidioc_querycap_capture,
 
-	.vidioc_enum_fmt_vid_cap	= fimc_vidioc_enum_fmt,
-	.vidioc_try_fmt_vid_cap		= fimc_vidioc_try_fmt,
-	.vidioc_s_fmt_vid_cap		= fimc_cap_s_fmt,
-	.vidioc_g_fmt_vid_cap		= fimc_vidioc_g_fmt,
+	.vidioc_enum_fmt_vid_cap_mplane	= fimc_vidioc_enum_fmt_mplane,
+	.vidioc_try_fmt_vid_cap_mplane	= fimc_vidioc_try_fmt_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= fimc_cap_s_fmt_mplane,
+	.vidioc_g_fmt_vid_cap_mplane	= fimc_vidioc_g_fmt_mplane,
 
 	.vidioc_reqbufs			= fimc_cap_reqbufs,
 	.vidioc_querybuf		= fimc_cap_querybuf,
@@ -770,6 +808,7 @@ static const struct v4l2_ioctl_ops fimc_capture_ioctl_ops = {
 	.vidioc_g_input			= fimc_cap_g_input,
 };
 
+/* fimc->lock must be already initialized */
 int fimc_register_capture_device(struct fimc_dev *fimc)
 {
 	struct v4l2_device *v4l2_dev = &fimc->vid_cap.v4l2_dev;
@@ -777,6 +816,8 @@ int fimc_register_capture_device(struct fimc_dev *fimc)
 	struct fimc_vid_cap *vid_cap;
 	struct fimc_ctx *ctx;
 	struct v4l2_format f;
+	struct fimc_frame *fr;
+	struct vb2_queue *q;
 	int ret;
 
 	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
@@ -788,8 +829,12 @@ int fimc_register_capture_device(struct fimc_dev *fimc)
 	ctx->out_path	 = FIMC_DMA;
 	ctx->state	 = FIMC_CTX_CAP;
 
-	f.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24;
-	ctx->d_frame.fmt = find_format(&f, FMT_FLAGS_M2M);
+	/* Default format of the output frames */
+	f.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB32;
+	fr = &ctx->d_frame;
+	fr->fmt = find_format(&f, FMT_FLAGS_M2M);
+	fr->width = fr->f_width = fr->o_width = 640;
+	fr->height = fr->f_height = fr->o_height = 480;
 
 	if (!v4l2_dev->name[0])
 		snprintf(v4l2_dev->name, sizeof(v4l2_dev->name),
@@ -812,6 +857,7 @@ int fimc_register_capture_device(struct fimc_dev *fimc)
 	vfd->ioctl_ops	= &fimc_capture_ioctl_ops;
 	vfd->minor	= -1;
 	vfd->release	= video_device_release;
+	vfd->lock	= &fimc->lock;
 	video_set_drvdata(vfd, fimc);
 
 	vid_cap = &fimc->vid_cap;
@@ -819,7 +865,7 @@ int fimc_register_capture_device(struct fimc_dev *fimc)
 	vid_cap->active_buf_cnt = 0;
 	vid_cap->reqbufs_count  = 0;
 	vid_cap->refcnt = 0;
-	/* The default color format for image sensor. */
+	/* Default color format for image sensor */
 	vid_cap->fmt.code = V4L2_MBUS_FMT_YUYV8_2X8;
 
 	INIT_LIST_HEAD(&vid_cap->pending_buf_q);
@@ -827,10 +873,16 @@ int fimc_register_capture_device(struct fimc_dev *fimc)
 	spin_lock_init(&ctx->slock);
 	vid_cap->ctx = ctx;
 
-	videobuf_queue_dma_contig_init(&vid_cap->vbq, &fimc_qops,
-		vid_cap->v4l2_dev.dev, &fimc->irqlock,
-		V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
-		sizeof(struct fimc_vid_buffer), (void *)ctx, NULL);
+	q = &fimc->vid_cap.vbq;
+	memset(q, 0, sizeof(*q));
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR;
+	q->drv_priv = fimc->vid_cap.ctx;
+	q->ops = &fimc_capture_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct fimc_vid_buffer);
+
+	vb2_queue_init(q);
 
 	ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
 	if (ret) {
diff --git a/drivers/media/video/s5p-fimc/fimc-core.c b/drivers/media/video/s5p-fimc/fimc-core.c
index 817aa66627f6..6c919b38a3d8 100644
--- a/drivers/media/video/s5p-fimc/fimc-core.c
+++ b/drivers/media/video/s5p-fimc/fimc-core.c
@@ -25,114 +25,141 @@
 #include <linux/slab.h>
 #include <linux/clk.h>
 #include <media/v4l2-ioctl.h>
-#include <media/videobuf-dma-contig.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
 
 #include "fimc-core.h"
 
-static char *fimc_clock_name[NUM_FIMC_CLOCKS] = { "sclk_fimc", "fimc" };
+static char *fimc_clocks[MAX_FIMC_CLOCKS] = {
+	"sclk_fimc", "fimc", "sclk_cam"
+};
 
 static struct fimc_fmt fimc_formats[] = {
 	{
-		.name	= "RGB565",
-		.fourcc	= V4L2_PIX_FMT_RGB565X,
-		.depth	= 16,
-		.color	= S5P_FIMC_RGB565,
-		.buff_cnt = 1,
-		.planes_cnt = 1,
-		.mbus_code = V4L2_MBUS_FMT_RGB565_2X8_BE,
-		.flags = FMT_FLAGS_M2M,
+		.name		= "RGB565",
+		.fourcc		= V4L2_PIX_FMT_RGB565X,
+		.depth		= { 16 },
+		.color		= S5P_FIMC_RGB565,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_RGB565_2X8_BE,
+		.flags		= FMT_FLAGS_M2M,
+	}, {
+		.name		= "BGR666",
+		.fourcc		= V4L2_PIX_FMT_BGR666,
+		.depth		= { 32 },
+		.color		= S5P_FIMC_RGB666,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.flags		= FMT_FLAGS_M2M,
+	}, {
+		.name		= "XRGB-8-8-8-8, 32 bpp",
+		.fourcc		= V4L2_PIX_FMT_RGB32,
+		.depth		= { 32 },
+		.color		= S5P_FIMC_RGB888,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.flags		= FMT_FLAGS_M2M,
+	}, {
+		.name		= "YUV 4:2:2 packed, YCbYCr",
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.depth		= { 16 },
+		.color		= S5P_FIMC_YCBYCR422,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_YUYV8_2X8,
+		.flags		= FMT_FLAGS_M2M | FMT_FLAGS_CAM,
 	}, {
-		.name	= "BGR666",
-		.fourcc	= V4L2_PIX_FMT_BGR666,
-		.depth	= 32,
-		.color	= S5P_FIMC_RGB666,
-		.buff_cnt = 1,
-		.planes_cnt = 1,
-		.flags = FMT_FLAGS_M2M,
+		.name		= "YUV 4:2:2 packed, CbYCrY",
+		.fourcc		= V4L2_PIX_FMT_UYVY,
+		.depth		= { 16 },
+		.color		= S5P_FIMC_CBYCRY422,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_UYVY8_2X8,
+		.flags		= FMT_FLAGS_M2M | FMT_FLAGS_CAM,
 	}, {
-		.name = "XRGB-8-8-8-8, 32 bpp",
-		.fourcc	= V4L2_PIX_FMT_RGB32,
-		.depth = 32,
-		.color	= S5P_FIMC_RGB888,
-		.buff_cnt = 1,
-		.planes_cnt = 1,
-		.flags = FMT_FLAGS_M2M,
+		.name		= "YUV 4:2:2 packed, CrYCbY",
+		.fourcc		= V4L2_PIX_FMT_VYUY,
+		.depth		= { 16 },
+		.color		= S5P_FIMC_CRYCBY422,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_VYUY8_2X8,
+		.flags		= FMT_FLAGS_M2M | FMT_FLAGS_CAM,
 	}, {
-		.name	= "YUV 4:2:2 packed, YCbYCr",
-		.fourcc	= V4L2_PIX_FMT_YUYV,
-		.depth	= 16,
-		.color	= S5P_FIMC_YCBYCR422,
-		.buff_cnt = 1,
-		.planes_cnt = 1,
-		.mbus_code = V4L2_MBUS_FMT_YUYV8_2X8,
-		.flags = FMT_FLAGS_M2M | FMT_FLAGS_CAM,
+		.name		= "YUV 4:2:2 packed, YCrYCb",
+		.fourcc		= V4L2_PIX_FMT_YVYU,
+		.depth		= { 16 },
+		.color		= S5P_FIMC_YCRYCB422,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_YVYU8_2X8,
+		.flags		= FMT_FLAGS_M2M | FMT_FLAGS_CAM,
 	}, {
-		.name	= "YUV 4:2:2 packed, CbYCrY",
-		.fourcc	= V4L2_PIX_FMT_UYVY,
-		.depth	= 16,
-		.color	= S5P_FIMC_CBYCRY422,
-		.buff_cnt = 1,
-		.planes_cnt = 1,
-		.mbus_code = V4L2_MBUS_FMT_UYVY8_2X8,
-		.flags = FMT_FLAGS_M2M | FMT_FLAGS_CAM,
+		.name		= "YUV 4:2:2 planar, Y/Cb/Cr",
+		.fourcc		= V4L2_PIX_FMT_YUV422P,
+		.depth		= { 12 },
+		.color		= S5P_FIMC_YCBYCR422,
+		.memplanes	= 1,
+		.colplanes	= 3,
+		.flags		= FMT_FLAGS_M2M,
 	}, {
-		.name	= "YUV 4:2:2 packed, CrYCbY",
-		.fourcc	= V4L2_PIX_FMT_VYUY,
-		.depth	= 16,
-		.color	= S5P_FIMC_CRYCBY422,
-		.buff_cnt = 1,
-		.planes_cnt = 1,
-		.mbus_code = V4L2_MBUS_FMT_VYUY8_2X8,
-		.flags = FMT_FLAGS_M2M | FMT_FLAGS_CAM,
+		.name		= "YUV 4:2:2 planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV16,
+		.depth		= { 16 },
+		.color		= S5P_FIMC_YCBYCR422,
+		.memplanes	= 1,
+		.colplanes	= 2,
+		.flags		= FMT_FLAGS_M2M,
 	}, {
-		.name	= "YUV 4:2:2 packed, YCrYCb",
-		.fourcc	= V4L2_PIX_FMT_YVYU,
-		.depth	= 16,
-		.color	= S5P_FIMC_YCRYCB422,
-		.buff_cnt = 1,
-		.planes_cnt = 1,
-		.mbus_code = V4L2_MBUS_FMT_YVYU8_2X8,
-		.flags = FMT_FLAGS_M2M | FMT_FLAGS_CAM,
+		.name		= "YUV 4:2:2 planar, Y/CrCb",
+		.fourcc		= V4L2_PIX_FMT_NV61,
+		.depth		= { 16 },
+		.color		= S5P_FIMC_YCRYCB422,
+		.memplanes	= 1,
+		.colplanes	= 2,
+		.flags		= FMT_FLAGS_M2M,
 	}, {
-		.name	= "YUV 4:2:2 planar, Y/Cb/Cr",
-		.fourcc	= V4L2_PIX_FMT_YUV422P,
-		.depth	= 12,
-		.color	= S5P_FIMC_YCBCR422,
-		.buff_cnt = 1,
-		.planes_cnt = 3,
-		.flags = FMT_FLAGS_M2M,
+		.name		= "YUV 4:2:0 planar, YCbCr",
+		.fourcc		= V4L2_PIX_FMT_YUV420,
+		.depth		= { 12 },
+		.color		= S5P_FIMC_YCBCR420,
+		.memplanes	= 1,
+		.colplanes	= 3,
+		.flags		= FMT_FLAGS_M2M,
 	}, {
-		.name	= "YUV 4:2:2 planar, Y/CbCr",
-		.fourcc	= V4L2_PIX_FMT_NV16,
-		.depth	= 16,
-		.color	= S5P_FIMC_YCBCR422,
-		.buff_cnt = 1,
-		.planes_cnt = 2,
-		.flags = FMT_FLAGS_M2M,
+		.name		= "YUV 4:2:0 planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV12,
+		.depth		= { 12 },
+		.color		= S5P_FIMC_YCBCR420,
+		.memplanes	= 1,
+		.colplanes	= 2,
+		.flags		= FMT_FLAGS_M2M,
 	}, {
-		.name	= "YUV 4:2:2 planar, Y/CrCb",
-		.fourcc	= V4L2_PIX_FMT_NV61,
-		.depth	= 16,
-		.color	= S5P_FIMC_RGB565,
-		.buff_cnt = 1,
-		.planes_cnt = 2,
-		.flags = FMT_FLAGS_M2M,
+		.name		= "YUV 4:2:0 non-contiguous 2-planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV12M,
+		.color		= S5P_FIMC_YCBCR420,
+		.depth		= { 8, 4 },
+		.memplanes	= 2,
+		.colplanes	= 2,
+		.flags		= FMT_FLAGS_M2M,
 	}, {
-		.name	= "YUV 4:2:0 planar, YCbCr",
-		.fourcc	= V4L2_PIX_FMT_YUV420,
-		.depth	= 12,
-		.color	= S5P_FIMC_YCBCR420,
-		.buff_cnt = 1,
-		.planes_cnt = 3,
-		.flags = FMT_FLAGS_M2M,
+		.name		= "YUV 4:2:0 non-contiguous 3-planar, Y/Cb/Cr",
+		.fourcc		= V4L2_PIX_FMT_YUV420M,
+		.color		= S5P_FIMC_YCBCR420,
+		.depth		= { 8, 2, 2 },
+		.memplanes	= 3,
+		.colplanes	= 3,
+		.flags		= FMT_FLAGS_M2M,
 	}, {
-		.name	= "YUV 4:2:0 planar, Y/CbCr",
-		.fourcc	= V4L2_PIX_FMT_NV12,
-		.depth	= 12,
-		.color	= S5P_FIMC_YCBCR420,
-		.buff_cnt = 1,
-		.planes_cnt = 2,
-		.flags = FMT_FLAGS_M2M,
+		.name		= "YUV 4:2:0 non-contiguous 2-planar, Y/CbCr, tiled",
+		.fourcc		= V4L2_PIX_FMT_NV12MT,
+		.color		= S5P_FIMC_YCBCR420,
+		.depth		= { 8, 4 },
+		.memplanes	= 2,
+		.colplanes	= 2,
+		.flags		= FMT_FLAGS_M2M,
 	},
 };
 
@@ -173,24 +200,21 @@ static struct v4l2_queryctrl *get_ctrl(int id)
 	return NULL;
 }
 
-int fimc_check_scaler_ratio(struct v4l2_rect *r, struct fimc_frame *f)
+int fimc_check_scaler_ratio(int sw, int sh, int dw, int dh, int rot)
 {
-	if (r->width > f->width) {
-		if (f->width > (r->width * SCALER_MAX_HRATIO))
-			return -EINVAL;
-	} else {
-		if ((f->width * SCALER_MAX_HRATIO) < r->width)
-			return -EINVAL;
-	}
+	int tx, ty;
 
-	if (r->height > f->height) {
-		if (f->height > (r->height * SCALER_MAX_VRATIO))
-			return -EINVAL;
+	if (rot == 90 || rot == 270) {
+		ty = dw;
+		tx = dh;
 	} else {
-		if ((f->height * SCALER_MAX_VRATIO) < r->height)
-			return -EINVAL;
+		tx = dw;
+		ty = dh;
 	}
 
+	if ((sw >= SCALER_MAX_HRATIO * tx) || (sh >= SCALER_MAX_VRATIO * ty))
+		return -EINVAL;
+
 	return 0;
 }
 
@@ -221,6 +245,7 @@ int fimc_set_scaler_info(struct fimc_ctx *ctx)
 	struct fimc_scaler *sc = &ctx->scaler;
 	struct fimc_frame *s_frame = &ctx->s_frame;
 	struct fimc_frame *d_frame = &ctx->d_frame;
+	struct samsung_fimc_variant *variant = ctx->fimc_dev->variant;
 	int tx, ty, sx, sy;
 	int ret;
 
@@ -259,8 +284,14 @@ int fimc_set_scaler_info(struct fimc_ctx *ctx)
 	sc->pre_dst_width = sx / sc->pre_hratio;
 	sc->pre_dst_height = sy / sc->pre_vratio;
 
-	sc->main_hratio = (sx << 8) / (tx << sc->hfactor);
-	sc->main_vratio = (sy << 8) / (ty << sc->vfactor);
+	if (variant->has_mainscaler_ext) {
+		sc->main_hratio = (sx << 14) / (tx << sc->hfactor);
+		sc->main_vratio = (sy << 14) / (ty << sc->vfactor);
+	} else {
+		sc->main_hratio = (sx << 8) / (tx << sc->hfactor);
+		sc->main_vratio = (sy << 8) / (ty << sc->vfactor);
+
+	}
 
 	sc->scaleup_h = (tx >= sx) ? 1 : 0;
 	sc->scaleup_v = (ty >= sy) ? 1 : 0;
@@ -276,14 +307,65 @@ int fimc_set_scaler_info(struct fimc_ctx *ctx)
 	return 0;
 }
 
-static void fimc_capture_handler(struct fimc_dev *fimc)
+static void fimc_m2m_job_finish(struct fimc_ctx *ctx, int vb_state)
+{
+	struct vb2_buffer *src_vb, *dst_vb;
+	struct fimc_dev *fimc = ctx->fimc_dev;
+
+	if (!ctx || !ctx->m2m_ctx)
+		return;
+
+	src_vb = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+	dst_vb = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
+
+	if (src_vb && dst_vb) {
+		v4l2_m2m_buf_done(src_vb, vb_state);
+		v4l2_m2m_buf_done(dst_vb, vb_state);
+		v4l2_m2m_job_finish(fimc->m2m.m2m_dev, ctx->m2m_ctx);
+	}
+}
+
+/* Complete the transaction which has been scheduled for execution. */
+static void fimc_m2m_shutdown(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	int ret;
+
+	if (!fimc_m2m_pending(fimc))
+		return;
+
+	fimc_ctx_state_lock_set(FIMC_CTX_SHUT, ctx);
+
+	ret = wait_event_timeout(fimc->irq_queue,
+			   !fimc_ctx_state_is_set(FIMC_CTX_SHUT, ctx),
+			   FIMC_SHUTDOWN_TIMEOUT);
+	/*
+	 * In case of a timeout the buffers are not released in the interrupt
+	 * handler so return them here with the error flag set, if there are
+	 * any on the queue.
+	 */
+	if (ret == 0)
+		fimc_m2m_job_finish(ctx, VB2_BUF_STATE_ERROR);
+}
+
+static int stop_streaming(struct vb2_queue *q)
+{
+	struct fimc_ctx *ctx = q->drv_priv;
+
+	fimc_m2m_shutdown(ctx);
+
+	return 0;
+}
+
+static void fimc_capture_irq_handler(struct fimc_dev *fimc)
 {
 	struct fimc_vid_cap *cap = &fimc->vid_cap;
-	struct fimc_vid_buffer *v_buf = NULL;
+	struct fimc_vid_buffer *v_buf;
 
-	if (!list_empty(&cap->active_buf_q)) {
+	if (!list_empty(&cap->active_buf_q) &&
+	    test_bit(ST_CAPT_RUN, &fimc->state)) {
 		v_buf = active_queue_pop(cap);
-		fimc_buf_finish(fimc, v_buf);
+		vb2_buffer_done(&v_buf->vb, VB2_BUF_STATE_DONE);
 	}
 
 	if (test_and_clear_bit(ST_CAPT_SHUT, &fimc->state)) {
@@ -297,13 +379,6 @@ static void fimc_capture_handler(struct fimc_dev *fimc)
 		fimc_hw_set_output_addr(fimc, &v_buf->paddr, cap->buf_index);
 		v_buf->index = cap->buf_index;
 
-		dbg("hw ptr: %d, sw ptr: %d",
-		    fimc_hw_get_frame_index(fimc), cap->buf_index);
-
-		spin_lock(&fimc->irqlock);
-		v_buf->vb.state = VIDEOBUF_ACTIVE;
-		spin_unlock(&fimc->irqlock);
-
 		/* Move the buffer to the capture active queue */
 		active_queue_add(cap, v_buf);
 
@@ -312,77 +387,79 @@ static void fimc_capture_handler(struct fimc_dev *fimc)
 
 		if (++cap->buf_index >= FIMC_MAX_OUT_BUFS)
 			cap->buf_index = 0;
+	}
 
-	} else if (test_and_clear_bit(ST_CAPT_STREAM, &fimc->state) &&
-		   cap->active_buf_cnt <= 1) {
-		fimc_deactivate_capture(fimc);
+	if (cap->active_buf_cnt == 0) {
+		clear_bit(ST_CAPT_RUN, &fimc->state);
+
+		if (++cap->buf_index >= FIMC_MAX_OUT_BUFS)
+			cap->buf_index = 0;
+	} else {
+		set_bit(ST_CAPT_RUN, &fimc->state);
 	}
 
-	dbg("frame: %d, active_buf_cnt= %d",
+	dbg("frame: %d, active_buf_cnt: %d",
 	    fimc_hw_get_frame_index(fimc), cap->active_buf_cnt);
 }
 
 static irqreturn_t fimc_isr(int irq, void *priv)
 {
-	struct fimc_vid_buffer *src_buf, *dst_buf;
-	struct fimc_ctx *ctx;
 	struct fimc_dev *fimc = priv;
+	struct fimc_vid_cap *cap = &fimc->vid_cap;
+	struct fimc_ctx *ctx;
 
-	BUG_ON(!fimc);
 	fimc_hw_clear_irq(fimc);
 
-	spin_lock(&fimc->slock);
-
 	if (test_and_clear_bit(ST_M2M_PEND, &fimc->state)) {
 		ctx = v4l2_m2m_get_curr_priv(fimc->m2m.m2m_dev);
-		if (!ctx || !ctx->m2m_ctx)
-			goto isr_unlock;
-		src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
-		dst_buf = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
-		if (src_buf && dst_buf) {
-			spin_lock(&fimc->irqlock);
-			src_buf->vb.state = dst_buf->vb.state = VIDEOBUF_DONE;
-			wake_up(&src_buf->vb.done);
-			wake_up(&dst_buf->vb.done);
-			spin_unlock(&fimc->irqlock);
-			v4l2_m2m_job_finish(fimc->m2m.m2m_dev, ctx->m2m_ctx);
+		if (ctx != NULL) {
+			fimc_m2m_job_finish(ctx, VB2_BUF_STATE_DONE);
+
+			spin_lock(&ctx->slock);
+			if (ctx->state & FIMC_CTX_SHUT) {
+				ctx->state &= ~FIMC_CTX_SHUT;
+				wake_up(&fimc->irq_queue);
+			}
+			spin_unlock(&ctx->slock);
 		}
-		goto isr_unlock;
 
+		return IRQ_HANDLED;
 	}
 
-	if (test_bit(ST_CAPT_RUN, &fimc->state))
-		fimc_capture_handler(fimc);
+	spin_lock(&fimc->slock);
 
-	if (test_and_clear_bit(ST_CAPT_PEND, &fimc->state)) {
-		set_bit(ST_CAPT_RUN, &fimc->state);
-		wake_up(&fimc->irq_queue);
+	if (test_bit(ST_CAPT_PEND, &fimc->state)) {
+		fimc_capture_irq_handler(fimc);
+
+		if (cap->active_buf_cnt == 1) {
+			fimc_deactivate_capture(fimc);
+			clear_bit(ST_CAPT_STREAM, &fimc->state);
+		}
 	}
 
-isr_unlock:
 	spin_unlock(&fimc->slock);
 	return IRQ_HANDLED;
 }
 
-/* The color format (planes_cnt, buff_cnt) must be already configured. */
-int fimc_prepare_addr(struct fimc_ctx *ctx, struct fimc_vid_buffer *buf,
+/* The color format (colplanes, memplanes) must be already configured. */
+int fimc_prepare_addr(struct fimc_ctx *ctx, struct vb2_buffer *vb,
 		      struct fimc_frame *frame, struct fimc_addr *paddr)
 {
 	int ret = 0;
 	u32 pix_size;
 
-	if (buf == NULL || frame == NULL)
+	if (vb == NULL || frame == NULL)
 		return -EINVAL;
 
 	pix_size = frame->width * frame->height;
 
-	dbg("buff_cnt= %d, planes_cnt= %d, frame->size= %d, pix_size= %d",
-		frame->fmt->buff_cnt, frame->fmt->planes_cnt,
-		frame->size, pix_size);
+	dbg("memplanes= %d, colplanes= %d, pix_size= %d",
+		frame->fmt->memplanes, frame->fmt->colplanes, pix_size);
+
+	paddr->y = vb2_dma_contig_plane_paddr(vb, 0);
 
-	if (frame->fmt->buff_cnt == 1) {
-		paddr->y = videobuf_to_dma_contig(&buf->vb);
-		switch (frame->fmt->planes_cnt) {
+	if (frame->fmt->memplanes == 1) {
+		switch (frame->fmt->colplanes) {
 		case 1:
 			paddr->cb = 0;
 			paddr->cr = 0;
@@ -405,6 +482,12 @@ int fimc_prepare_addr(struct fimc_ctx *ctx, struct fimc_vid_buffer *buf,
 		default:
 			return -EINVAL;
 		}
+	} else {
+		if (frame->fmt->memplanes >= 2)
+			paddr->cb = vb2_dma_contig_plane_paddr(vb, 1);
+
+		if (frame->fmt->memplanes == 3)
+			paddr->cr = vb2_dma_contig_plane_paddr(vb, 2);
 	}
 
 	dbg("PHYS_ADDR: y= 0x%X  cb= 0x%X cr= 0x%X ret= %d",
@@ -423,34 +506,34 @@ static void fimc_set_yuv_order(struct fimc_ctx *ctx)
 	/* Set order for 1 plane input formats. */
 	switch (ctx->s_frame.fmt->color) {
 	case S5P_FIMC_YCRYCB422:
-		ctx->in_order_1p = S5P_FIMC_IN_YCRYCB;
+		ctx->in_order_1p = S5P_MSCTRL_ORDER422_CBYCRY;
 		break;
 	case S5P_FIMC_CBYCRY422:
-		ctx->in_order_1p = S5P_FIMC_IN_CBYCRY;
+		ctx->in_order_1p = S5P_MSCTRL_ORDER422_YCRYCB;
 		break;
 	case S5P_FIMC_CRYCBY422:
-		ctx->in_order_1p = S5P_FIMC_IN_CRYCBY;
+		ctx->in_order_1p = S5P_MSCTRL_ORDER422_YCBYCR;
 		break;
 	case S5P_FIMC_YCBYCR422:
 	default:
-		ctx->in_order_1p = S5P_FIMC_IN_YCBYCR;
+		ctx->in_order_1p = S5P_MSCTRL_ORDER422_CRYCBY;
 		break;
 	}
 	dbg("ctx->in_order_1p= %d", ctx->in_order_1p);
 
 	switch (ctx->d_frame.fmt->color) {
 	case S5P_FIMC_YCRYCB422:
-		ctx->out_order_1p = S5P_FIMC_OUT_YCRYCB;
+		ctx->out_order_1p = S5P_CIOCTRL_ORDER422_CBYCRY;
 		break;
 	case S5P_FIMC_CBYCRY422:
-		ctx->out_order_1p = S5P_FIMC_OUT_CBYCRY;
+		ctx->out_order_1p = S5P_CIOCTRL_ORDER422_YCRYCB;
 		break;
 	case S5P_FIMC_CRYCBY422:
-		ctx->out_order_1p = S5P_FIMC_OUT_CRYCBY;
+		ctx->out_order_1p = S5P_CIOCTRL_ORDER422_YCBYCR;
 		break;
 	case S5P_FIMC_YCBYCR422:
 	default:
-		ctx->out_order_1p = S5P_FIMC_OUT_YCBYCR;
+		ctx->out_order_1p = S5P_CIOCTRL_ORDER422_CRYCBY;
 		break;
 	}
 	dbg("ctx->out_order_1p= %d", ctx->out_order_1p);
@@ -459,10 +542,14 @@ static void fimc_set_yuv_order(struct fimc_ctx *ctx)
 static void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 {
 	struct samsung_fimc_variant *variant = ctx->fimc_dev->variant;
+	u32 i, depth = 0;
+
+	for (i = 0; i < f->fmt->colplanes; i++)
+		depth += f->fmt->depth[i];
 
 	f->dma_offset.y_h = f->offs_h;
 	if (!variant->pix_hoff)
-		f->dma_offset.y_h *= (f->fmt->depth >> 3);
+		f->dma_offset.y_h *= (depth >> 3);
 
 	f->dma_offset.y_v = f->offs_v;
 
@@ -473,7 +560,7 @@ static void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 	f->dma_offset.cr_v = f->offs_v;
 
 	if (!variant->pix_hoff) {
-		if (f->fmt->planes_cnt == 3) {
+		if (f->fmt->colplanes == 3) {
 			f->dma_offset.cb_h >>= 1;
 			f->dma_offset.cr_h >>= 1;
 		}
@@ -499,7 +586,7 @@ static void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 int fimc_prepare_config(struct fimc_ctx *ctx, u32 flags)
 {
 	struct fimc_frame *s_frame, *d_frame;
-	struct fimc_vid_buffer *buf = NULL;
+	struct vb2_buffer *vb = NULL;
 	int ret = 0;
 
 	s_frame = &ctx->s_frame;
@@ -522,15 +609,15 @@ int fimc_prepare_config(struct fimc_ctx *ctx, u32 flags)
 	ctx->scaler.enabled = 1;
 
 	if (flags & FIMC_SRC_ADDR) {
-		buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
-		ret = fimc_prepare_addr(ctx, buf, s_frame, &s_frame->paddr);
+		vb = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+		ret = fimc_prepare_addr(ctx, vb, s_frame, &s_frame->paddr);
 		if (ret)
 			return ret;
 	}
 
 	if (flags & FIMC_DST_ADDR) {
-		buf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
-		ret = fimc_prepare_addr(ctx, buf, d_frame, &d_frame->paddr);
+		vb = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+		ret = fimc_prepare_addr(ctx, vb, d_frame, &d_frame->paddr);
 	}
 
 	return ret;
@@ -553,26 +640,28 @@ static void fimc_dma_run(void *priv)
 
 	ctx->state |= (FIMC_SRC_ADDR | FIMC_DST_ADDR);
 	ret = fimc_prepare_config(ctx, ctx->state);
-	if (ret) {
-		err("Wrong parameters");
+	if (ret)
 		goto dma_unlock;
-	}
+
 	/* Reconfigure hardware if the context has changed. */
 	if (fimc->m2m.ctx != ctx) {
 		ctx->state |= FIMC_PARAMS;
 		fimc->m2m.ctx = ctx;
 	}
 
+	spin_lock(&fimc->slock);
 	fimc_hw_set_input_addr(fimc, &ctx->s_frame.paddr);
 
 	if (ctx->state & FIMC_PARAMS) {
 		fimc_hw_set_input_path(ctx);
 		fimc_hw_set_in_dma(ctx);
-		if (fimc_set_scaler_info(ctx)) {
-			err("Scaler setup error");
+		ret = fimc_set_scaler_info(ctx);
+		if (ret) {
+			spin_unlock(&fimc->slock);
 			goto dma_unlock;
 		}
-		fimc_hw_set_scaler(ctx);
+		fimc_hw_set_prescaler(ctx);
+		fimc_hw_set_mainscaler(ctx);
 		fimc_hw_set_target_format(ctx);
 		fimc_hw_set_rotation(ctx);
 		fimc_hw_set_effect(ctx);
@@ -587,8 +676,10 @@ static void fimc_dma_run(void *priv)
 
 	fimc_activate_capture(ctx);
 
-	ctx->state &= (FIMC_CTX_M2M | FIMC_CTX_CAP);
+	ctx->state &= (FIMC_CTX_M2M | FIMC_CTX_CAP |
+		       FIMC_SRC_FMT | FIMC_DST_FMT);
 	fimc_hw_activate_input_dma(fimc, true);
+	spin_unlock(&fimc->slock);
 
 dma_unlock:
 	spin_unlock_irqrestore(&ctx->slock, flags);
@@ -596,109 +687,84 @@ dma_unlock:
 
 static void fimc_job_abort(void *priv)
 {
-	/* Nothing done in job_abort. */
+	fimc_m2m_shutdown(priv);
 }
 
-static void fimc_buf_release(struct videobuf_queue *vq,
-				    struct videobuf_buffer *vb)
+static int fimc_queue_setup(struct vb2_queue *vq, unsigned int *num_buffers,
+			    unsigned int *num_planes, unsigned long sizes[],
+			    void *allocators[])
 {
-	videobuf_dma_contig_free(vq, vb);
-	vb->state = VIDEOBUF_NEEDS_INIT;
-}
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
+	struct fimc_frame *f;
+	int i;
 
-static int fimc_buf_setup(struct videobuf_queue *vq, unsigned int *count,
-				unsigned int *size)
-{
-	struct fimc_ctx *ctx = vq->priv_data;
-	struct fimc_frame *frame;
+	f = ctx_get_frame(ctx, vq->type);
+	if (IS_ERR(f))
+		return PTR_ERR(f);
 
-	frame = ctx_get_frame(ctx, vq->type);
-	if (IS_ERR(frame))
-		return PTR_ERR(frame);
+	/*
+	 * Return number of non-contigous planes (plane buffers)
+	 * depending on the configured color format.
+	 */
+	if (f->fmt)
+		*num_planes = f->fmt->memplanes;
+
+	for (i = 0; i < f->fmt->memplanes; i++) {
+		sizes[i] = (f->width * f->height * f->fmt->depth[i]) >> 3;
+		allocators[i] = ctx->fimc_dev->alloc_ctx;
+	}
+
+	if (*num_buffers == 0)
+		*num_buffers = 1;
 
-	*size = (frame->width * frame->height * frame->fmt->depth) >> 3;
-	if (0 == *count)
-		*count = 1;
 	return 0;
 }
 
-static int fimc_buf_prepare(struct videobuf_queue *vq,
-		struct videobuf_buffer *vb, enum v4l2_field field)
+static int fimc_buf_prepare(struct vb2_buffer *vb)
 {
-	struct fimc_ctx *ctx = vq->priv_data;
-	struct v4l2_device *v4l2_dev = &ctx->fimc_dev->m2m.v4l2_dev;
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 	struct fimc_frame *frame;
-	int ret;
+	int i;
 
-	frame = ctx_get_frame(ctx, vq->type);
+	frame = ctx_get_frame(ctx, vb->vb2_queue->type);
 	if (IS_ERR(frame))
 		return PTR_ERR(frame);
 
-	if (vb->baddr) {
-		if (vb->bsize < frame->size) {
-			v4l2_err(v4l2_dev,
-				"User-provided buffer too small (%d < %d)\n",
-				 vb->bsize, frame->size);
-			WARN_ON(1);
-			return -EINVAL;
-		}
-	} else if (vb->state != VIDEOBUF_NEEDS_INIT
-		   && vb->bsize < frame->size) {
-		return -EINVAL;
-	}
-
-	vb->width       = frame->width;
-	vb->height      = frame->height;
-	vb->bytesperline = (frame->width * frame->fmt->depth) >> 3;
-	vb->size        = frame->size;
-	vb->field       = field;
-
-	if (VIDEOBUF_NEEDS_INIT == vb->state) {
-		ret = videobuf_iolock(vq, vb, NULL);
-		if (ret) {
-			v4l2_err(v4l2_dev, "Iolock failed\n");
-			fimc_buf_release(vq, vb);
-			return ret;
-		}
-	}
-	vb->state = VIDEOBUF_PREPARED;
+	for (i = 0; i < frame->fmt->memplanes; i++)
+		vb2_set_plane_payload(vb, i, frame->payload[i]);
 
 	return 0;
 }
 
-static void fimc_buf_queue(struct videobuf_queue *vq,
-				  struct videobuf_buffer *vb)
+static void fimc_buf_queue(struct vb2_buffer *vb)
 {
-	struct fimc_ctx *ctx = vq->priv_data;
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
-	unsigned long flags;
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 
 	dbg("ctx: %p, ctx->state: 0x%x", ctx, ctx->state);
 
-	if ((ctx->state & FIMC_CTX_M2M) && ctx->m2m_ctx) {
-		v4l2_m2m_buf_queue(ctx->m2m_ctx, vq, vb);
-	} else if (ctx->state & FIMC_CTX_CAP) {
-		spin_lock_irqsave(&fimc->slock, flags);
-		fimc_vid_cap_buf_queue(fimc, (struct fimc_vid_buffer *)vb);
+	if (ctx->m2m_ctx)
+		v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
+}
 
-		dbg("fimc->cap.active_buf_cnt: %d",
-		    fimc->vid_cap.active_buf_cnt);
+static void fimc_lock(struct vb2_queue *vq)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
+	mutex_lock(&ctx->fimc_dev->lock);
+}
 
-		if (cap->active_buf_cnt >= cap->reqbufs_count ||
-		   cap->active_buf_cnt >= FIMC_MAX_OUT_BUFS) {
-			if (!test_and_set_bit(ST_CAPT_STREAM, &fimc->state))
-				fimc_activate_capture(ctx);
-		}
-		spin_unlock_irqrestore(&fimc->slock, flags);
-	}
+static void fimc_unlock(struct vb2_queue *vq)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
+	mutex_unlock(&ctx->fimc_dev->lock);
 }
 
-struct videobuf_queue_ops fimc_qops = {
-	.buf_setup	= fimc_buf_setup,
-	.buf_prepare	= fimc_buf_prepare,
-	.buf_queue	= fimc_buf_queue,
-	.buf_release	= fimc_buf_release,
+struct vb2_ops fimc_qops = {
+	.queue_setup	 = fimc_queue_setup,
+	.buf_prepare	 = fimc_buf_prepare,
+	.buf_queue	 = fimc_buf_queue,
+	.wait_prepare	 = fimc_unlock,
+	.wait_finish	 = fimc_lock,
+	.stop_streaming	 = stop_streaming,
 };
 
 static int fimc_m2m_querycap(struct file *file, void *priv,
@@ -712,12 +778,13 @@ static int fimc_m2m_querycap(struct file *file, void *priv,
 	cap->bus_info[0] = 0;
 	cap->version = KERNEL_VERSION(1, 0, 0);
 	cap->capabilities = V4L2_CAP_STREAMING |
-		V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT;
+		V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT |
+		V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
 
 	return 0;
 }
 
-int fimc_vidioc_enum_fmt(struct file *file, void *priv,
+int fimc_vidioc_enum_fmt_mplane(struct file *file, void *priv,
 				struct v4l2_fmtdesc *f)
 {
 	struct fimc_fmt *fmt;
@@ -732,25 +799,39 @@ int fimc_vidioc_enum_fmt(struct file *file, void *priv,
 	return 0;
 }
 
-int fimc_vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
+int fimc_vidioc_g_fmt_mplane(struct file *file, void *priv,
+			     struct v4l2_format *f)
 {
 	struct fimc_ctx *ctx = priv;
-	struct fimc_dev *fimc = ctx->fimc_dev;
 	struct fimc_frame *frame;
+	struct v4l2_pix_format_mplane *pixm;
+	int i;
 
 	frame = ctx_get_frame(ctx, f->type);
 	if (IS_ERR(frame))
 		return PTR_ERR(frame);
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
+	pixm = &f->fmt.pix_mp;
+
+	pixm->width		= frame->width;
+	pixm->height		= frame->height;
+	pixm->field		= V4L2_FIELD_NONE;
+	pixm->pixelformat	= frame->fmt->fourcc;
+	pixm->colorspace	= V4L2_COLORSPACE_JPEG;
+	pixm->num_planes	= frame->fmt->memplanes;
+
+	for (i = 0; i < pixm->num_planes; ++i) {
+		int bpl = frame->o_width;
 
-	f->fmt.pix.width	= frame->width;
-	f->fmt.pix.height	= frame->height;
-	f->fmt.pix.field	= V4L2_FIELD_NONE;
-	f->fmt.pix.pixelformat	= frame->fmt->fourcc;
+		if (frame->fmt->colplanes == 1) /* packed formats */
+			bpl = (bpl * frame->fmt->depth[0]) / 8;
+
+		pixm->plane_fmt[i].bytesperline = bpl;
+
+		pixm->plane_fmt[i].sizeimage = (frame->o_width *
+			frame->o_height * frame->fmt->depth[i]) / 8;
+	}
 
-	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
@@ -785,42 +866,40 @@ struct fimc_fmt *find_mbus_format(struct v4l2_mbus_framefmt *f,
 }
 
 
-int fimc_vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
+int fimc_vidioc_try_fmt_mplane(struct file *file, void *priv,
+			       struct v4l2_format *f)
 {
 	struct fimc_ctx *ctx = priv;
 	struct fimc_dev *fimc = ctx->fimc_dev;
 	struct samsung_fimc_variant *variant = fimc->variant;
-	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
 	struct fimc_fmt *fmt;
 	u32 max_width, mod_x, mod_y, mask;
-	int ret = -EINVAL, is_output = 0;
+	int i, is_output = 0;
+
 
-	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
-		if (ctx->state & FIMC_CTX_CAP)
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		if (fimc_ctx_state_is_set(FIMC_CTX_CAP, ctx))
 			return -EINVAL;
 		is_output = 1;
-	} else if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+	} else if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
 		return -EINVAL;
 	}
 
-	dbg("w: %d, h: %d, bpl: %d",
-	    pix->width, pix->height, pix->bytesperline);
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
+	dbg("w: %d, h: %d", pix->width, pix->height);
 
 	mask = is_output ? FMT_FLAGS_M2M : FMT_FLAGS_M2M | FMT_FLAGS_CAM;
 	fmt = find_format(f, mask);
 	if (!fmt) {
 		v4l2_err(&fimc->m2m.v4l2_dev, "Fourcc format (0x%X) invalid.\n",
 			 pix->pixelformat);
-		goto tf_out;
+		return -EINVAL;
 	}
 
 	if (pix->field == V4L2_FIELD_ANY)
 		pix->field = V4L2_FIELD_NONE;
 	else if (V4L2_FIELD_NONE != pix->field)
-		goto tf_out;
+		return -EINVAL;
 
 	if (is_output) {
 		max_width = variant->pix_limit->scaler_dis_w;
@@ -834,7 +913,7 @@ int fimc_vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
 		mod_x = 6; /* 64 x 32 pixels tile */
 		mod_y = 5;
 	} else {
-		if (fimc->id == 1 && fimc->variant->pix_hoff)
+		if (fimc->id == 1 && variant->pix_hoff)
 			mod_y = fimc_fmt_is_rgb(fmt->color) ? 0 : 1;
 		else
 			mod_y = mod_x;
@@ -845,74 +924,72 @@ int fimc_vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
 	v4l_bound_align_image(&pix->width, 16, max_width, mod_x,
 		&pix->height, 8, variant->pix_limit->scaler_dis_w, mod_y, 0);
 
-	if (pix->bytesperline == 0 ||
-	    (pix->bytesperline * 8 / fmt->depth) > pix->width)
-		pix->bytesperline = (pix->width * fmt->depth) >> 3;
+	pix->num_planes = fmt->memplanes;
+	pix->colorspace	= V4L2_COLORSPACE_JPEG;
 
-	if (pix->sizeimage == 0)
-		pix->sizeimage = pix->height * pix->bytesperline;
+	for (i = 0; i < pix->num_planes; ++i) {
+		int bpl = pix->plane_fmt[i].bytesperline;
 
-	dbg("w: %d, h: %d, bpl: %d, depth: %d",
-	    pix->width, pix->height, pix->bytesperline, fmt->depth);
+		dbg("[%d] bpl: %d, depth: %d, w: %d, h: %d",
+		    i, bpl, fmt->depth[i], pix->width, pix->height);
 
-	ret = 0;
+		if (!bpl || (bpl * 8 / fmt->depth[i]) > pix->width)
+			bpl = (pix->width * fmt->depth[0]) >> 3;
 
-tf_out:
-	mutex_unlock(&fimc->lock);
-	return ret;
+		if (!pix->plane_fmt[i].sizeimage)
+			pix->plane_fmt[i].sizeimage = pix->height * bpl;
+
+		pix->plane_fmt[i].bytesperline = bpl;
+
+		dbg("[%d]: bpl: %d, sizeimage: %d",
+		    i, pix->plane_fmt[i].bytesperline,
+		    pix->plane_fmt[i].sizeimage);
+	}
+
+	return 0;
 }
 
-static int fimc_m2m_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
+static int fimc_m2m_s_fmt_mplane(struct file *file, void *priv,
+				 struct v4l2_format *f)
 {
 	struct fimc_ctx *ctx = priv;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct v4l2_device *v4l2_dev = &fimc->m2m.v4l2_dev;
-	struct videobuf_queue *vq;
+	struct vb2_queue *vq;
 	struct fimc_frame *frame;
-	struct v4l2_pix_format *pix;
-	unsigned long flags;
-	int ret = 0;
+	struct v4l2_pix_format_mplane *pix;
+	int i, ret = 0;
 
-	ret = fimc_vidioc_try_fmt(file, priv, f);
+	ret = fimc_vidioc_try_fmt_mplane(file, priv, f);
 	if (ret)
 		return ret;
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
 	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
-	mutex_lock(&vq->vb_lock);
 
-	if (videobuf_queue_is_busy(vq)) {
-		v4l2_err(v4l2_dev, "%s: queue (%d) busy\n", __func__, f->type);
-		ret = -EBUSY;
-		goto sf_out;
+	if (vb2_is_streaming(vq)) {
+		v4l2_err(&fimc->m2m.v4l2_dev, "queue (%d) busy\n", f->type);
+		return -EBUSY;
 	}
 
-	spin_lock_irqsave(&ctx->slock, flags);
-	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 		frame = &ctx->s_frame;
-		ctx->state |= FIMC_SRC_FMT;
-	} else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
 		frame = &ctx->d_frame;
-		ctx->state |= FIMC_DST_FMT;
 	} else {
-		spin_unlock_irqrestore(&ctx->slock, flags);
-		v4l2_err(&ctx->fimc_dev->m2m.v4l2_dev,
+		v4l2_err(&fimc->m2m.v4l2_dev,
 			 "Wrong buffer/video queue type (%d)\n", f->type);
-		ret = -EINVAL;
-		goto sf_out;
+		return -EINVAL;
 	}
-	spin_unlock_irqrestore(&ctx->slock, flags);
 
-	pix = &f->fmt.pix;
+	pix = &f->fmt.pix_mp;
 	frame->fmt = find_format(f, FMT_FLAGS_M2M);
-	if (!frame->fmt) {
-		ret = -EINVAL;
-		goto sf_out;
-	}
+	if (!frame->fmt)
+		return -EINVAL;
 
-	frame->f_width	= pix->bytesperline * 8 / frame->fmt->depth;
+	for (i = 0; i < frame->fmt->colplanes; i++)
+		frame->payload[i] = pix->plane_fmt[i].bytesperline * pix->height;
+
+	frame->f_width	= pix->plane_fmt[0].bytesperline * 8 /
+		frame->fmt->depth[0];
 	frame->f_height	= pix->height;
 	frame->width	= pix->width;
 	frame->height	= pix->height;
@@ -920,19 +997,15 @@ static int fimc_m2m_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
 	frame->o_height = pix->height;
 	frame->offs_h	= 0;
 	frame->offs_v	= 0;
-	frame->size	= (pix->width * pix->height * frame->fmt->depth) >> 3;
-	vq->field	= pix->field;
 
-	spin_lock_irqsave(&ctx->slock, flags);
-	ctx->state |= FIMC_PARAMS;
-	spin_unlock_irqrestore(&ctx->slock, flags);
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		fimc_ctx_state_lock_set(FIMC_PARAMS | FIMC_DST_FMT, ctx);
+	else
+		fimc_ctx_state_lock_set(FIMC_PARAMS | FIMC_SRC_FMT, ctx);
 
 	dbg("f_w: %d, f_h: %d", frame->f_width, frame->f_height);
 
-sf_out:
-	mutex_unlock(&vq->vb_lock);
-	mutex_unlock(&fimc->lock);
-	return ret;
+	return 0;
 }
 
 static int fimc_m2m_reqbufs(struct file *file, void *priv,
@@ -968,6 +1041,15 @@ static int fimc_m2m_streamon(struct file *file, void *priv,
 			   enum v4l2_buf_type type)
 {
 	struct fimc_ctx *ctx = priv;
+
+	/* The source and target color format need to be set */
+	if (V4L2_TYPE_IS_OUTPUT(type)) {
+		if (!fimc_ctx_state_is_set(FIMC_SRC_FMT, ctx))
+			return -EINVAL;
+	} else if (!fimc_ctx_state_is_set(FIMC_DST_FMT, ctx)) {
+		return -EINVAL;
+	}
+
 	return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
 }
 
@@ -991,12 +1073,9 @@ int fimc_vidioc_queryctrl(struct file *file, void *priv,
 		return 0;
 	}
 
-	if (ctx->state & FIMC_CTX_CAP) {
-		if (mutex_lock_interruptible(&ctx->fimc_dev->lock))
-			return -ERESTARTSYS;
-		ret = v4l2_subdev_call(ctx->fimc_dev->vid_cap.sd,
+	if (fimc_ctx_state_is_set(FIMC_CTX_CAP, ctx)) {
+		return v4l2_subdev_call(ctx->fimc_dev->vid_cap.sd,
 					core, queryctrl, qc);
-		mutex_unlock(&ctx->fimc_dev->lock);
 	}
 	return ret;
 }
@@ -1006,10 +1085,6 @@ int fimc_vidioc_g_ctrl(struct file *file, void *priv,
 {
 	struct fimc_ctx *ctx = priv;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	int ret = 0;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
 
 	switch (ctrl->id) {
 	case V4L2_CID_HFLIP:
@@ -1022,19 +1097,17 @@ int fimc_vidioc_g_ctrl(struct file *file, void *priv,
 		ctrl->value = ctx->rotation;
 		break;
 	default:
-		if (ctx->state & FIMC_CTX_CAP) {
-			ret = v4l2_subdev_call(fimc->vid_cap.sd, core,
-				       g_ctrl, ctrl);
+		if (fimc_ctx_state_is_set(FIMC_CTX_CAP, ctx)) {
+			return v4l2_subdev_call(fimc->vid_cap.sd, core,
+						g_ctrl, ctrl);
 		} else {
-			v4l2_err(&fimc->m2m.v4l2_dev,
-				 "Invalid control\n");
-			ret = -EINVAL;
+			v4l2_err(&fimc->m2m.v4l2_dev, "Invalid control\n");
+			return -EINVAL;
 		}
 	}
 	dbg("ctrl->value= %d", ctrl->value);
 
-	mutex_unlock(&fimc->lock);
-	return ret;
+	return 0;
 }
 
 int check_ctrl_val(struct fimc_ctx *ctx,  struct v4l2_control *ctrl)
@@ -1058,16 +1131,7 @@ int fimc_s_ctrl(struct fimc_ctx *ctx, struct v4l2_control *ctrl)
 {
 	struct samsung_fimc_variant *variant = ctx->fimc_dev->variant;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	unsigned long flags;
-
-	if (ctx->rotation != 0 &&
-	    (ctrl->id == V4L2_CID_HFLIP || ctrl->id == V4L2_CID_VFLIP)) {
-		v4l2_err(&fimc->m2m.v4l2_dev,
-			 "Simultaneous flip and rotation is not supported\n");
-		return -EINVAL;
-	}
-
-	spin_lock_irqsave(&ctx->slock, flags);
+	int ret = 0;
 
 	switch (ctrl->id) {
 	case V4L2_CID_HFLIP:
@@ -1085,29 +1149,36 @@ int fimc_s_ctrl(struct fimc_ctx *ctx, struct v4l2_control *ctrl)
 		break;
 
 	case V4L2_CID_ROTATE:
+		if (fimc_ctx_state_is_set(FIMC_DST_FMT | FIMC_SRC_FMT, ctx)) {
+			ret = fimc_check_scaler_ratio(ctx->s_frame.width,
+					ctx->s_frame.height, ctx->d_frame.width,
+					ctx->d_frame.height, ctrl->value);
+		}
+
+		if (ret) {
+			v4l2_err(&fimc->m2m.v4l2_dev, "Out of scaler range\n");
+			return -EINVAL;
+		}
+
 		/* Check for the output rotator availability */
 		if ((ctrl->value == 90 || ctrl->value == 270) &&
-		    (ctx->in_path == FIMC_DMA && !variant->has_out_rot)) {
-			spin_unlock_irqrestore(&ctx->slock, flags);
+		    (ctx->in_path == FIMC_DMA && !variant->has_out_rot))
 			return -EINVAL;
-		} else {
-			ctx->rotation = ctrl->value;
-		}
+		ctx->rotation = ctrl->value;
 		break;
 
 	default:
-		spin_unlock_irqrestore(&ctx->slock, flags);
 		v4l2_err(&fimc->m2m.v4l2_dev, "Invalid control\n");
 		return -EINVAL;
 	}
-	ctx->state |= FIMC_PARAMS;
-	spin_unlock_irqrestore(&ctx->slock, flags);
+
+	fimc_ctx_state_lock_set(FIMC_PARAMS, ctx);
 
 	return 0;
 }
 
 static int fimc_m2m_s_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
+			   struct v4l2_control *ctrl)
 {
 	struct fimc_ctx *ctx = priv;
 	int ret = 0;
@@ -1125,22 +1196,17 @@ static int fimc_m2m_cropcap(struct file *file, void *fh,
 {
 	struct fimc_frame *frame;
 	struct fimc_ctx *ctx = fh;
-	struct fimc_dev *fimc = ctx->fimc_dev;
 
 	frame = ctx_get_frame(ctx, cr->type);
 	if (IS_ERR(frame))
 		return PTR_ERR(frame);
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
 	cr->bounds.left		= 0;
 	cr->bounds.top		= 0;
 	cr->bounds.width	= frame->f_width;
 	cr->bounds.height	= frame->f_height;
 	cr->defrect		= cr->bounds;
 
-	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
@@ -1148,21 +1214,16 @@ static int fimc_m2m_g_crop(struct file *file, void *fh, struct v4l2_crop *cr)
 {
 	struct fimc_frame *frame;
 	struct fimc_ctx *ctx = file->private_data;
-	struct fimc_dev *fimc = ctx->fimc_dev;
 
 	frame = ctx_get_frame(ctx, cr->type);
 	if (IS_ERR(frame))
 		return PTR_ERR(frame);
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
 	cr->c.left = frame->offs_h;
 	cr->c.top = frame->offs_v;
 	cr->c.width = frame->width;
 	cr->c.height = frame->height;
 
-	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
@@ -1170,7 +1231,9 @@ int fimc_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
 {
 	struct fimc_dev *fimc = ctx->fimc_dev;
 	struct fimc_frame *f;
-	u32 min_size, halign;
+	u32 min_size, halign, depth = 0;
+	bool is_capture_ctx;
+	int i;
 
 	if (cr->c.top < 0 || cr->c.left < 0) {
 		v4l2_err(&fimc->m2m.v4l2_dev,
@@ -1178,10 +1241,12 @@ int fimc_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
 		return -EINVAL;
 	}
 
-	if (cr->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		f = (ctx->state & FIMC_CTX_CAP) ? &ctx->s_frame : &ctx->d_frame;
-	else if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT &&
-		 ctx->state & FIMC_CTX_M2M)
+	is_capture_ctx = fimc_ctx_state_is_set(FIMC_CTX_CAP, ctx);
+
+	if (cr->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		f = is_capture_ctx ? &ctx->s_frame : &ctx->d_frame;
+	else if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
+		 !is_capture_ctx)
 		f = &ctx->s_frame;
 	else
 		return -EINVAL;
@@ -1189,21 +1254,24 @@ int fimc_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
 	min_size = (f == &ctx->s_frame) ?
 		fimc->variant->min_inp_pixsize : fimc->variant->min_out_pixsize;
 
-	if (ctx->state & FIMC_CTX_M2M) {
+	/* Get pixel alignment constraints. */
+	if (is_capture_ctx) {
+		min_size = 16;
+		halign = 4;
+	} else {
 		if (fimc->id == 1 && fimc->variant->pix_hoff)
 			halign = fimc_fmt_is_rgb(f->fmt->color) ? 0 : 1;
 		else
 			halign = ffs(min_size) - 1;
-	/* there are more strict aligment requirements at camera interface */
-	} else {
-		min_size = 16;
-		halign = 4;
 	}
 
+	for (i = 0; i < f->fmt->colplanes; i++)
+		depth += f->fmt->depth[i];
+
 	v4l_bound_align_image(&cr->c.width, min_size, f->o_width,
 			      ffs(min_size) - 1,
 			      &cr->c.height, min_size, f->o_height,
-			      halign, 64/(ALIGN(f->fmt->depth, 8)));
+			      halign, 64/(ALIGN(depth, 8)));
 
 	/* adjust left/top if cropping rectangle is out of bounds */
 	if (cr->c.left + cr->c.width > f->o_width)
@@ -1212,8 +1280,7 @@ int fimc_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
 		cr->c.top = f->o_height - cr->c.height;
 
 	cr->c.left = round_down(cr->c.left, min_size);
-	cr->c.top  = round_down(cr->c.top,
-				ctx->state & FIMC_CTX_M2M ? 8 : 16);
+	cr->c.top  = round_down(cr->c.top, is_capture_ctx ? 16 : 8);
 
 	dbg("l:%d, t:%d, w:%d, h:%d, f_w: %d, f_h: %d",
 	    cr->c.left, cr->c.top, cr->c.width, cr->c.height,
@@ -1222,12 +1289,10 @@ int fimc_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
 	return 0;
 }
 
-
 static int fimc_m2m_s_crop(struct file *file, void *fh, struct v4l2_crop *cr)
 {
 	struct fimc_ctx *ctx = file->private_data;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	unsigned long flags;
 	struct fimc_frame *f;
 	int ret;
 
@@ -1235,52 +1300,52 @@ static int fimc_m2m_s_crop(struct file *file, void *fh, struct v4l2_crop *cr)
 	if (ret)
 		return ret;
 
-	f = (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) ?
+	f = (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ?
 		&ctx->s_frame : &ctx->d_frame;
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
-	spin_lock_irqsave(&ctx->slock, flags);
-	if (~ctx->state & (FIMC_SRC_FMT | FIMC_DST_FMT)) {
-		/* Check to see if scaling ratio is within supported range */
-		if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
-			ret = fimc_check_scaler_ratio(&cr->c, &ctx->d_frame);
-		else
-			ret = fimc_check_scaler_ratio(&cr->c, &ctx->s_frame);
+	/* Check to see if scaling ratio is within supported range */
+	if (fimc_ctx_state_is_set(FIMC_DST_FMT | FIMC_SRC_FMT, ctx)) {
+		if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+			ret = fimc_check_scaler_ratio(cr->c.width, cr->c.height,
+						      ctx->d_frame.width,
+						      ctx->d_frame.height,
+						      ctx->rotation);
+		} else {
+			ret = fimc_check_scaler_ratio(ctx->s_frame.width,
+						      ctx->s_frame.height,
+						      cr->c.width, cr->c.height,
+						      ctx->rotation);
+		}
 		if (ret) {
-			v4l2_err(&fimc->m2m.v4l2_dev, "Out of scaler range");
-			ret = -EINVAL;
-			goto scr_unlock;
+			v4l2_err(&fimc->m2m.v4l2_dev, "Out of scaler range\n");
+			return -EINVAL;
 		}
 	}
-	ctx->state |= FIMC_PARAMS;
 
 	f->offs_h = cr->c.left;
 	f->offs_v = cr->c.top;
 	f->width  = cr->c.width;
 	f->height = cr->c.height;
 
-scr_unlock:
-	spin_unlock_irqrestore(&ctx->slock, flags);
-	mutex_unlock(&fimc->lock);
+	fimc_ctx_state_lock_set(FIMC_PARAMS, ctx);
+
 	return 0;
 }
 
 static const struct v4l2_ioctl_ops fimc_m2m_ioctl_ops = {
 	.vidioc_querycap		= fimc_m2m_querycap,
 
-	.vidioc_enum_fmt_vid_cap	= fimc_vidioc_enum_fmt,
-	.vidioc_enum_fmt_vid_out	= fimc_vidioc_enum_fmt,
+	.vidioc_enum_fmt_vid_cap_mplane	= fimc_vidioc_enum_fmt_mplane,
+	.vidioc_enum_fmt_vid_out_mplane	= fimc_vidioc_enum_fmt_mplane,
 
-	.vidioc_g_fmt_vid_cap		= fimc_vidioc_g_fmt,
-	.vidioc_g_fmt_vid_out		= fimc_vidioc_g_fmt,
+	.vidioc_g_fmt_vid_cap_mplane	= fimc_vidioc_g_fmt_mplane,
+	.vidioc_g_fmt_vid_out_mplane	= fimc_vidioc_g_fmt_mplane,
 
-	.vidioc_try_fmt_vid_cap		= fimc_vidioc_try_fmt,
-	.vidioc_try_fmt_vid_out		= fimc_vidioc_try_fmt,
+	.vidioc_try_fmt_vid_cap_mplane	= fimc_vidioc_try_fmt_mplane,
+	.vidioc_try_fmt_vid_out_mplane	= fimc_vidioc_try_fmt_mplane,
 
-	.vidioc_s_fmt_vid_cap		= fimc_m2m_s_fmt,
-	.vidioc_s_fmt_vid_out		= fimc_m2m_s_fmt,
+	.vidioc_s_fmt_vid_cap_mplane	= fimc_m2m_s_fmt_mplane,
+	.vidioc_s_fmt_vid_out_mplane	= fimc_m2m_s_fmt_mplane,
 
 	.vidioc_reqbufs			= fimc_m2m_reqbufs,
 	.vidioc_querybuf		= fimc_m2m_querybuf,
@@ -1301,26 +1366,39 @@ static const struct v4l2_ioctl_ops fimc_m2m_ioctl_ops = {
 
 };
 
-static void queue_init(void *priv, struct videobuf_queue *vq,
-		       enum v4l2_buf_type type)
+static int queue_init(void *priv, struct vb2_queue *src_vq,
+		      struct vb2_queue *dst_vq)
 {
 	struct fimc_ctx *ctx = priv;
-	struct fimc_dev *fimc = ctx->fimc_dev;
+	int ret;
+
+	memset(src_vq, 0, sizeof(*src_vq));
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	src_vq->drv_priv = ctx;
+	src_vq->ops = &fimc_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
 
-	videobuf_queue_dma_contig_init(vq, &fimc_qops,
-		&fimc->pdev->dev,
-		&fimc->irqlock, type, V4L2_FIELD_NONE,
-		sizeof(struct fimc_vid_buffer), priv, NULL);
+	memset(dst_vq, 0, sizeof(*dst_vq));
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	dst_vq->drv_priv = ctx;
+	dst_vq->ops = &fimc_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+
+	return vb2_queue_init(dst_vq);
 }
 
 static int fimc_m2m_open(struct file *file)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
 	struct fimc_ctx *ctx = NULL;
-	int err = 0;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
 
 	dbg("pid: %d, state: 0x%lx, refcnt: %d",
 		task_pid_nr(current), fimc->state, fimc->vid_cap.refcnt);
@@ -1329,19 +1407,15 @@ static int fimc_m2m_open(struct file *file)
 	 * Return if the corresponding video capture node
 	 * is already opened.
 	 */
-	if (fimc->vid_cap.refcnt > 0) {
-		err = -EBUSY;
-		goto err_unlock;
-	}
+	if (fimc->vid_cap.refcnt > 0)
+		return -EBUSY;
 
 	fimc->m2m.refcnt++;
 	set_bit(ST_OUTDMA_RUN, &fimc->state);
 
 	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
-	if (!ctx) {
-		err = -ENOMEM;
-		goto err_unlock;
-	}
+	if (!ctx)
+		return -ENOMEM;
 
 	file->private_data = ctx;
 	ctx->fimc_dev = fimc;
@@ -1355,15 +1429,14 @@ static int fimc_m2m_open(struct file *file)
 	ctx->out_path = FIMC_DMA;
 	spin_lock_init(&ctx->slock);
 
-	ctx->m2m_ctx = v4l2_m2m_ctx_init(ctx, fimc->m2m.m2m_dev, queue_init);
+	ctx->m2m_ctx = v4l2_m2m_ctx_init(fimc->m2m.m2m_dev, ctx, queue_init);
 	if (IS_ERR(ctx->m2m_ctx)) {
-		err = PTR_ERR(ctx->m2m_ctx);
+		int err = PTR_ERR(ctx->m2m_ctx);
 		kfree(ctx);
+		return err;
 	}
 
-err_unlock:
-	mutex_unlock(&fimc->lock);
-	return err;
+	return 0;
 }
 
 static int fimc_m2m_release(struct file *file)
@@ -1371,8 +1444,6 @@ static int fimc_m2m_release(struct file *file)
 	struct fimc_ctx *ctx = file->private_data;
 	struct fimc_dev *fimc = ctx->fimc_dev;
 
-	mutex_lock(&fimc->lock);
-
 	dbg("pid: %d, state: 0x%lx, refcnt= %d",
 		task_pid_nr(current), fimc->state, fimc->m2m.refcnt);
 
@@ -1381,7 +1452,6 @@ static int fimc_m2m_release(struct file *file)
 	if (--fimc->m2m.refcnt <= 0)
 		clear_bit(ST_OUTDMA_RUN, &fimc->state);
 
-	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
@@ -1415,7 +1485,6 @@ static struct v4l2_m2m_ops m2m_ops = {
 	.job_abort	= fimc_job_abort,
 };
 
-
 static int fimc_register_m2m_device(struct fimc_dev *fimc)
 {
 	struct video_device *vfd;
@@ -1448,6 +1517,7 @@ static int fimc_register_m2m_device(struct fimc_dev *fimc)
 	vfd->ioctl_ops	= &fimc_m2m_ioctl_ops;
 	vfd->minor	= -1;
 	vfd->release	= video_device_release;
+	vfd->lock	= &fimc->lock;
 
 	snprintf(vfd->name, sizeof(vfd->name), "%s:m2m", dev_name(&pdev->dev));
 
@@ -1496,7 +1566,7 @@ static void fimc_unregister_m2m_device(struct fimc_dev *fimc)
 static void fimc_clk_release(struct fimc_dev *fimc)
 {
 	int i;
-	for (i = 0; i < NUM_FIMC_CLOCKS; i++) {
+	for (i = 0; i < fimc->num_clocks; i++) {
 		if (fimc->clock[i]) {
 			clk_disable(fimc->clock[i]);
 			clk_put(fimc->clock[i]);
@@ -1507,15 +1577,16 @@ static void fimc_clk_release(struct fimc_dev *fimc)
 static int fimc_clk_get(struct fimc_dev *fimc)
 {
 	int i;
-	for (i = 0; i < NUM_FIMC_CLOCKS; i++) {
-		fimc->clock[i] = clk_get(&fimc->pdev->dev, fimc_clock_name[i]);
-		if (IS_ERR(fimc->clock[i])) {
-			dev_err(&fimc->pdev->dev,
-				"failed to get fimc clock: %s\n",
-				fimc_clock_name[i]);
-			return -ENXIO;
+	for (i = 0; i < fimc->num_clocks; i++) {
+		fimc->clock[i] = clk_get(&fimc->pdev->dev, fimc_clocks[i]);
+
+		if (!IS_ERR_OR_NULL(fimc->clock[i])) {
+			clk_enable(fimc->clock[i]);
+			continue;
 		}
-		clk_enable(fimc->clock[i]);
+		dev_err(&fimc->pdev->dev, "failed to get fimc clock: %s\n",
+			fimc_clocks[i]);
+		return -ENXIO;
 	}
 	return 0;
 }
@@ -1525,7 +1596,9 @@ static int fimc_probe(struct platform_device *pdev)
 	struct fimc_dev *fimc;
 	struct resource *res;
 	struct samsung_fimc_driverdata *drv_data;
+	struct s5p_platform_fimc *pdata;
 	int ret = 0;
+	int cap_input_index = -1;
 
 	dev_dbg(&pdev->dev, "%s():\n", __func__);
 
@@ -1545,10 +1618,10 @@ static int fimc_probe(struct platform_device *pdev)
 	fimc->id = pdev->id;
 	fimc->variant = drv_data->variant[fimc->id];
 	fimc->pdev = pdev;
-	fimc->pdata = pdev->dev.platform_data;
+	pdata = pdev->dev.platform_data;
+	fimc->pdata = pdata;
 	fimc->state = ST_IDLE;
 
-	spin_lock_init(&fimc->irqlock);
 	init_waitqueue_head(&fimc->irq_queue);
 	spin_lock_init(&fimc->slock);
 
@@ -1576,10 +1649,18 @@ static int fimc_probe(struct platform_device *pdev)
 		goto err_req_region;
 	}
 
+	fimc->num_clocks = MAX_FIMC_CLOCKS - 1;
+
+	/* Check if a video capture node needs to be registered. */
+	if (pdata && pdata->num_clients > 0) {
+		cap_input_index = 0;
+		fimc->num_clocks++;
+	}
+
 	ret = fimc_clk_get(fimc);
 	if (ret)
 		goto err_regs_unmap;
-	clk_set_rate(fimc->clock[0], drv_data->lclk_frequency);
+	clk_set_rate(fimc->clock[CLK_BUS], drv_data->lclk_frequency);
 
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (!res) {
@@ -1597,24 +1678,24 @@ static int fimc_probe(struct platform_device *pdev)
 		goto err_clk;
 	}
 
+	/* Initialize contiguous memory allocator */
+	fimc->alloc_ctx = vb2_dma_contig_init_ctx(&fimc->pdev->dev);
+	if (IS_ERR(fimc->alloc_ctx)) {
+		ret = PTR_ERR(fimc->alloc_ctx);
+		goto err_irq;
+	}
+
 	ret = fimc_register_m2m_device(fimc);
 	if (ret)
 		goto err_irq;
 
 	/* At least one camera sensor is required to register capture node */
-	if (fimc->pdata) {
-		int i;
-		for (i = 0; i < FIMC_MAX_CAMIF_CLIENTS; ++i)
-			if (fimc->pdata->isp_info[i])
-				break;
-
-		if (i < FIMC_MAX_CAMIF_CLIENTS) {
-			ret = fimc_register_capture_device(fimc);
-			if (ret)
-				goto err_m2m;
-		}
+	if (cap_input_index >= 0) {
+		ret = fimc_register_capture_device(fimc);
+		if (ret)
+			goto err_m2m;
+		clk_disable(fimc->clock[CLK_CAM]);
 	}
-
 	/*
 	 * Exclude the additional output DMA address registers by masking
 	 * them out on HW revisions that provide extended capabilites.
@@ -1656,6 +1737,9 @@ static int __devexit fimc_remove(struct platform_device *pdev)
 	fimc_unregister_capture_device(fimc);
 
 	fimc_clk_release(fimc);
+
+	vb2_dma_contig_cleanup_ctx(fimc->alloc_ctx);
+
 	iounmap(fimc->regs);
 	release_resource(fimc->regs_res);
 	kfree(fimc->regs_res);
@@ -1726,6 +1810,7 @@ static struct samsung_fimc_variant fimc1_variant_s5pv210 = {
 	.pix_hoff	 = 1,
 	.has_inp_rot	 = 1,
 	.has_out_rot	 = 1,
+	.has_mainscaler_ext = 1,
 	.min_inp_pixsize = 16,
 	.min_out_pixsize = 16,
 	.hor_offs_align	 = 1,
@@ -1747,6 +1832,7 @@ static struct samsung_fimc_variant fimc0_variant_s5pv310 = {
 	.has_inp_rot	 = 1,
 	.has_out_rot	 = 1,
 	.has_cistatus2	 = 1,
+	.has_mainscaler_ext = 1,
 	.min_inp_pixsize = 16,
 	.min_out_pixsize = 16,
 	.hor_offs_align	 = 1,
@@ -1757,6 +1843,7 @@ static struct samsung_fimc_variant fimc0_variant_s5pv310 = {
 static struct samsung_fimc_variant fimc2_variant_s5pv310 = {
 	.pix_hoff	 = 1,
 	.has_cistatus2	 = 1,
+	.has_mainscaler_ext = 1,
 	.min_inp_pixsize = 16,
 	.min_out_pixsize = 16,
 	.hor_offs_align	 = 1,
diff --git a/drivers/media/video/s5p-fimc/fimc-core.h b/drivers/media/video/s5p-fimc/fimc-core.h
index 4f047d35f8ad..3beb1e5320ce 100644
--- a/drivers/media/video/s5p-fimc/fimc-core.h
+++ b/drivers/media/video/s5p-fimc/fimc-core.h
@@ -14,29 +14,27 @@
 /*#define DEBUG*/
 
 #include <linux/sched.h>
+#include <linux/spinlock.h>
 #include <linux/types.h>
 #include <linux/videodev2.h>
-#include <media/videobuf-core.h>
+#include <linux/io.h>
+#include <media/videobuf2-core.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-mem2mem.h>
 #include <media/v4l2-mediabus.h>
-#include <media/s3c_fimc.h>
+#include <media/s5p_fimc.h>
 
 #include "regs-fimc.h"
 
 #define err(fmt, args...) \
 	printk(KERN_ERR "%s:%d: " fmt "\n", __func__, __LINE__, ##args)
 
-#ifdef DEBUG
 #define dbg(fmt, args...) \
-	printk(KERN_DEBUG "%s:%d: " fmt "\n", __func__, __LINE__, ##args)
-#else
-#define dbg(fmt, args...)
-#endif
+	pr_debug("%s:%d: " fmt "\n", __func__, __LINE__, ##args)
 
 /* Time to wait for next frame VSYNC interrupt while stopping operation. */
 #define FIMC_SHUTDOWN_TIMEOUT	((100*HZ)/1000)
-#define NUM_FIMC_CLOCKS		2
+#define MAX_FIMC_CLOCKS		3
 #define MODULE_NAME		"s5p-fimc"
 #define FIMC_MAX_DEVS		4
 #define FIMC_MAX_OUT_BUFS	4
@@ -44,7 +42,13 @@
 #define SCALER_MAX_VRATIO	64
 #define DMA_MIN_SIZE		8
 
-/* FIMC device state flags */
+/* indices to the clocks array */
+enum {
+	CLK_BUS,
+	CLK_GATE,
+	CLK_CAM,
+};
+
 enum fimc_dev_flags {
 	/* for m2m node */
 	ST_IDLE,
@@ -63,20 +67,6 @@ enum fimc_dev_flags {
 #define fimc_capture_running(dev) test_bit(ST_CAPT_RUN, &(dev)->state)
 #define fimc_capture_pending(dev) test_bit(ST_CAPT_PEND, &(dev)->state)
 
-#define fimc_capture_active(dev) \
-	(test_bit(ST_CAPT_RUN, &(dev)->state) || \
-	 test_bit(ST_CAPT_PEND, &(dev)->state))
-
-#define fimc_capture_streaming(dev) \
-	test_bit(ST_CAPT_STREAM, &(dev)->state)
-
-#define fimc_buf_finish(dev, vid_buf) do { \
-	spin_lock(&(dev)->irqlock); \
-	(vid_buf)->vb.state = VIDEOBUF_DONE; \
-	spin_unlock(&(dev)->irqlock); \
-	wake_up(&(vid_buf)->vb.done); \
-} while (0)
-
 enum fimc_datapath {
 	FIMC_CAMERA,
 	FIMC_DMA,
@@ -90,7 +80,6 @@ enum fimc_color_fmt {
 	S5P_FIMC_RGB888,
 	S5P_FIMC_RGB30_LOCAL,
 	S5P_FIMC_YCBCR420 = 0x20,
-	S5P_FIMC_YCBCR422,
 	S5P_FIMC_YCBYCR422,
 	S5P_FIMC_YCRYCB422,
 	S5P_FIMC_CBYCRY422,
@@ -100,18 +89,6 @@ enum fimc_color_fmt {
 
 #define fimc_fmt_is_rgb(x) ((x) & 0x10)
 
-/* Y/Cb/Cr components order at DMA output for 1 plane YCbCr 4:2:2 formats. */
-#define	S5P_FIMC_OUT_CRYCBY	S5P_CIOCTRL_ORDER422_CRYCBY
-#define	S5P_FIMC_OUT_CBYCRY	S5P_CIOCTRL_ORDER422_YCRYCB
-#define	S5P_FIMC_OUT_YCRYCB	S5P_CIOCTRL_ORDER422_CBYCRY
-#define	S5P_FIMC_OUT_YCBYCR	S5P_CIOCTRL_ORDER422_YCBYCR
-
-/* Input Y/Cb/Cr components order for 1 plane YCbCr 4:2:2 color formats. */
-#define	S5P_FIMC_IN_CRYCBY	S5P_MSCTRL_ORDER422_CRYCBY
-#define	S5P_FIMC_IN_CBYCRY	S5P_MSCTRL_ORDER422_YCRYCB
-#define	S5P_FIMC_IN_YCRYCB	S5P_MSCTRL_ORDER422_CBYCRY
-#define	S5P_FIMC_IN_YCBYCR	S5P_MSCTRL_ORDER422_YCBYCR
-
 /* Cb/Cr chrominance components order for 2 plane Y/CbCr 4:2:2 formats. */
 #define	S5P_FIMC_LSB_CRCB	S5P_CIOCTRL_ORDER422_2P_LSB_CRCB
 
@@ -131,6 +108,7 @@ enum fimc_color_fmt {
 #define	FIMC_DST_FMT		(1 << 4)
 #define	FIMC_CTX_M2M		(1 << 5)
 #define	FIMC_CTX_CAP		(1 << 6)
+#define	FIMC_CTX_SHUT		(1 << 7)
 
 /* Image conversion flags */
 #define	FIMC_IN_DMA_ACCESS_TILED	(1 << 0)
@@ -157,18 +135,18 @@ enum fimc_color_fmt {
  * @name: format description
  * @fourcc: the fourcc code for this format, 0 if not applicable
  * @color: the corresponding fimc_color_fmt
- * @depth: driver's private 'number of bits per pixel'
- * @buff_cnt: number of physically non-contiguous data planes
- * @planes_cnt: number of physically contiguous data planes
+ * @depth: per plane driver's private 'number of bits per pixel'
+ * @memplanes: number of physically non-contiguous data planes
+ * @colplanes: number of physically contiguous data planes
  */
 struct fimc_fmt {
 	enum v4l2_mbus_pixelcode mbus_code;
 	char	*name;
 	u32	fourcc;
 	u32	color;
-	u16	buff_cnt;
-	u16	planes_cnt;
-	u16	depth;
+	u16	memplanes;
+	u16	colplanes;
+	u8	depth[VIDEO_MAX_PLANES];
 	u16	flags;
 #define FMT_FLAGS_CAM	(1 << 0)
 #define FMT_FLAGS_M2M	(1 << 1)
@@ -260,7 +238,8 @@ struct fimc_addr {
  * @index: buffer index for the output DMA engine
  */
 struct fimc_vid_buffer {
-	struct videobuf_buffer	vb;
+	struct vb2_buffer	vb;
+	struct list_head	list;
 	struct fimc_addr	paddr;
 	int			index;
 };
@@ -277,7 +256,7 @@ struct fimc_vid_buffer {
  * @height:	image pixel weight
  * @paddr:	image frame buffer physical addresses
  * @buf_cnt:	number of buffers depending on a color format
- * @size:	image size in bytes
+ * @payload:	image size in bytes (w x h x bpp)
  * @color:	color format
  * @dma_offset:	DMA offset in bytes
  */
@@ -290,7 +269,7 @@ struct fimc_frame {
 	u32	offs_v;
 	u32	width;
 	u32	height;
-	u32	size;
+	unsigned long		payload[VIDEO_MAX_PLANES];
 	struct fimc_addr	paddr;
 	struct fimc_dma_offset	dma_offset;
 	struct fimc_fmt		*fmt;
@@ -331,13 +310,14 @@ struct fimc_m2m_device {
  */
 struct fimc_vid_cap {
 	struct fimc_ctx			*ctx;
+	struct vb2_alloc_ctx		*alloc_ctx;
 	struct video_device		*vfd;
 	struct v4l2_device		v4l2_dev;
-	struct v4l2_subdev		*sd;
+	struct v4l2_subdev		*sd;;
 	struct v4l2_mbus_framefmt	fmt;
 	struct list_head		pending_buf_q;
 	struct list_head		active_buf_q;
-	struct videobuf_queue		vbq;
+	struct vb2_queue		vbq;
 	int				active_buf_cnt;
 	int				buf_index;
 	unsigned int			frame_count;
@@ -372,6 +352,8 @@ struct fimc_pix_limit {
  * @has_inp_rot: set if has input rotator
  * @has_out_rot: set if has output rotator
  * @has_cistatus2: 1 if CISTATUS2 register is present in this IP revision
+ * @has_mainscaler_ext: 1 if extended mainscaler ratios in CIEXTEN register
+ *			 are present in this IP revision
  * @pix_limit: pixel size constraints for the scaler
  * @min_inp_pixsize: minimum input pixel size
  * @min_out_pixsize: minimum output pixel size
@@ -383,6 +365,7 @@ struct samsung_fimc_variant {
 	unsigned int	has_inp_rot:1;
 	unsigned int	has_out_rot:1;
 	unsigned int	has_cistatus2:1;
+	unsigned int	has_mainscaler_ext:1;
 	struct fimc_pix_limit *pix_limit;
 	u16		min_inp_pixsize;
 	u16		min_out_pixsize;
@@ -412,12 +395,12 @@ struct fimc_ctx;
  * @lock:	the mutex protecting this data structure
  * @pdev:	pointer to the FIMC platform device
  * @pdata:	pointer to the device platform data
- * @id:		FIMC device index (0..2)
+ * @id:		FIMC device index (0..FIMC_MAX_DEVS)
+ * @num_clocks: the number of clocks managed by this device instance
  * @clock[]:	the clocks required for FIMC operation
  * @regs:	the mapped hardware registers
  * @regs_res:	the resource claimed for IO registers
  * @irq:	interrupt number of the FIMC subdevice
- * @irqlock:	spinlock protecting videobuffer queue
  * @irq_queue:
  * @m2m:	memory-to-memory V4L2 device information
  * @vid_cap:	camera capture device information
@@ -427,18 +410,19 @@ struct fimc_dev {
 	spinlock_t			slock;
 	struct mutex			lock;
 	struct platform_device		*pdev;
-	struct s3c_platform_fimc	*pdata;
+	struct s5p_platform_fimc	*pdata;
 	struct samsung_fimc_variant	*variant;
-	int				id;
-	struct clk			*clock[NUM_FIMC_CLOCKS];
+	u16				id;
+	u16				num_clocks;
+	struct clk			*clock[MAX_FIMC_CLOCKS];
 	void __iomem			*regs;
 	struct resource			*regs_res;
 	int				irq;
-	spinlock_t			irqlock;
 	wait_queue_head_t		irq_queue;
 	struct fimc_m2m_device		m2m;
 	struct fimc_vid_cap		vid_cap;
 	unsigned long			state;
+	struct vb2_alloc_ctx		*alloc_ctx;
 };
 
 /**
@@ -482,11 +466,41 @@ struct fimc_ctx {
 	struct v4l2_m2m_ctx	*m2m_ctx;
 };
 
-extern struct videobuf_queue_ops fimc_qops;
+static inline bool fimc_capture_active(struct fimc_dev *fimc)
+{
+	unsigned long flags;
+	bool ret;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	ret = !!(fimc->state & (1 << ST_CAPT_RUN) ||
+		 fimc->state & (1 << ST_CAPT_PEND));
+	spin_unlock_irqrestore(&fimc->slock, flags);
+	return ret;
+}
+
+static inline void fimc_ctx_state_lock_set(u32 state, struct fimc_ctx *ctx)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->slock, flags);
+	ctx->state |= state;
+	spin_unlock_irqrestore(&ctx->slock, flags);
+}
+
+static inline bool fimc_ctx_state_is_set(u32 mask, struct fimc_ctx *ctx)
+{
+	unsigned long flags;
+	bool ret;
+
+	spin_lock_irqsave(&ctx->slock, flags);
+	ret = (ctx->state & mask) == mask;
+	spin_unlock_irqrestore(&ctx->slock, flags);
+	return ret;
+}
 
 static inline int tiled_fmt(struct fimc_fmt *fmt)
 {
-	return 0;
+	return fmt->fourcc == V4L2_PIX_FMT_NV12MT;
 }
 
 static inline void fimc_hw_clear_irq(struct fimc_dev *dev)
@@ -542,12 +556,12 @@ static inline struct fimc_frame *ctx_get_frame(struct fimc_ctx *ctx,
 {
 	struct fimc_frame *frame;
 
-	if (V4L2_BUF_TYPE_VIDEO_OUTPUT == type) {
-		if (ctx->state & FIMC_CTX_M2M)
+	if (V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE == type) {
+		if (fimc_ctx_state_is_set(FIMC_CTX_M2M, ctx))
 			frame = &ctx->s_frame;
 		else
 			return ERR_PTR(-EINVAL);
-	} else if (V4L2_BUF_TYPE_VIDEO_CAPTURE == type) {
+	} else if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == type) {
 		frame = &ctx->d_frame;
 	} else {
 		v4l2_err(&ctx->fimc_dev->m2m.v4l2_dev,
@@ -581,7 +595,8 @@ void fimc_hw_set_target_format(struct fimc_ctx *ctx);
 void fimc_hw_set_out_dma(struct fimc_ctx *ctx);
 void fimc_hw_en_lastirq(struct fimc_dev *fimc, int enable);
 void fimc_hw_en_irq(struct fimc_dev *fimc, int enable);
-void fimc_hw_set_scaler(struct fimc_ctx *ctx);
+void fimc_hw_set_prescaler(struct fimc_ctx *ctx);
+void fimc_hw_set_mainscaler(struct fimc_ctx *ctx);
 void fimc_hw_en_capture(struct fimc_ctx *ctx);
 void fimc_hw_set_effect(struct fimc_ctx *ctx);
 void fimc_hw_set_in_dma(struct fimc_ctx *ctx);
@@ -589,23 +604,23 @@ void fimc_hw_set_input_path(struct fimc_ctx *ctx);
 void fimc_hw_set_output_path(struct fimc_ctx *ctx);
 void fimc_hw_set_input_addr(struct fimc_dev *fimc, struct fimc_addr *paddr);
 void fimc_hw_set_output_addr(struct fimc_dev *fimc, struct fimc_addr *paddr,
-			      int index);
+			     int index);
 int fimc_hw_set_camera_source(struct fimc_dev *fimc,
-			      struct s3c_fimc_isp_info *cam);
+			      struct s5p_fimc_isp_info *cam);
 int fimc_hw_set_camera_offset(struct fimc_dev *fimc, struct fimc_frame *f);
 int fimc_hw_set_camera_polarity(struct fimc_dev *fimc,
-				struct s3c_fimc_isp_info *cam);
+				struct s5p_fimc_isp_info *cam);
 int fimc_hw_set_camera_type(struct fimc_dev *fimc,
-			    struct s3c_fimc_isp_info *cam);
+			    struct s5p_fimc_isp_info *cam);
 
 /* -----------------------------------------------------*/
 /* fimc-core.c */
-int fimc_vidioc_enum_fmt(struct file *file, void *priv,
-		      struct v4l2_fmtdesc *f);
-int fimc_vidioc_g_fmt(struct file *file, void *priv,
-		      struct v4l2_format *f);
-int fimc_vidioc_try_fmt(struct file *file, void *priv,
-			struct v4l2_format *f);
+int fimc_vidioc_enum_fmt_mplane(struct file *file, void *priv,
+				struct v4l2_fmtdesc *f);
+int fimc_vidioc_g_fmt_mplane(struct file *file, void *priv,
+			     struct v4l2_format *f);
+int fimc_vidioc_try_fmt_mplane(struct file *file, void *priv,
+			       struct v4l2_format *f);
 int fimc_vidioc_queryctrl(struct file *file, void *priv,
 			  struct v4l2_queryctrl *qc);
 int fimc_vidioc_g_ctrl(struct file *file, void *priv,
@@ -619,10 +634,10 @@ struct fimc_fmt *find_format(struct v4l2_format *f, unsigned int mask);
 struct fimc_fmt *find_mbus_format(struct v4l2_mbus_framefmt *f,
 				  unsigned int mask);
 
-int fimc_check_scaler_ratio(struct v4l2_rect *r, struct fimc_frame *f);
+int fimc_check_scaler_ratio(int sw, int sh, int dw, int dh, int rot);
 int fimc_set_scaler_info(struct fimc_ctx *ctx);
 int fimc_prepare_config(struct fimc_ctx *ctx, u32 flags);
-int fimc_prepare_addr(struct fimc_ctx *ctx, struct fimc_vid_buffer *buf,
+int fimc_prepare_addr(struct fimc_ctx *ctx, struct vb2_buffer *vb,
 		      struct fimc_frame *frame, struct fimc_addr *paddr);
 
 /* -----------------------------------------------------*/
@@ -649,28 +664,27 @@ static inline void fimc_deactivate_capture(struct fimc_dev *fimc)
 }
 
 /*
- * Add video buffer to the active buffers queue.
- * The caller holds irqlock spinlock.
+ * Add buf to the capture active buffers queue.
+ * Locking: Need to be called with fimc_dev::slock held.
  */
 static inline void active_queue_add(struct fimc_vid_cap *vid_cap,
-					 struct fimc_vid_buffer *buf)
+				    struct fimc_vid_buffer *buf)
 {
-	buf->vb.state = VIDEOBUF_ACTIVE;
-	list_add_tail(&buf->vb.queue, &vid_cap->active_buf_q);
+	list_add_tail(&buf->list, &vid_cap->active_buf_q);
 	vid_cap->active_buf_cnt++;
 }
 
 /*
  * Pop a video buffer from the capture active buffers queue
- * Locking: Need to be called with dev->slock held.
+ * Locking: Need to be called with fimc_dev::slock held.
  */
 static inline struct fimc_vid_buffer *
 active_queue_pop(struct fimc_vid_cap *vid_cap)
 {
 	struct fimc_vid_buffer *buf;
 	buf = list_entry(vid_cap->active_buf_q.next,
-			 struct fimc_vid_buffer, vb.queue);
-	list_del(&buf->vb.queue);
+			 struct fimc_vid_buffer, list);
+	list_del(&buf->list);
 	vid_cap->active_buf_cnt--;
 	return buf;
 }
@@ -679,8 +693,7 @@ active_queue_pop(struct fimc_vid_cap *vid_cap)
 static inline void fimc_pending_queue_add(struct fimc_vid_cap *vid_cap,
 					  struct fimc_vid_buffer *buf)
 {
-	buf->vb.state = VIDEOBUF_QUEUED;
-	list_add_tail(&buf->vb.queue, &vid_cap->pending_buf_q);
+	list_add_tail(&buf->list, &vid_cap->pending_buf_q);
 }
 
 /* Add video buffer to the capture pending buffers queue */
@@ -689,10 +702,9 @@ pending_queue_pop(struct fimc_vid_cap *vid_cap)
 {
 	struct fimc_vid_buffer *buf;
 	buf = list_entry(vid_cap->pending_buf_q.next,
-			struct fimc_vid_buffer, vb.queue);
-	list_del(&buf->vb.queue);
+			struct fimc_vid_buffer, list);
+	list_del(&buf->list);
 	return buf;
 }
 
-
 #endif /* FIMC_CORE_H_ */
diff --git a/drivers/media/video/s5p-fimc/fimc-reg.c b/drivers/media/video/s5p-fimc/fimc-reg.c
index 511631a2e5c3..4893b2d91d84 100644
--- a/drivers/media/video/s5p-fimc/fimc-reg.c
+++ b/drivers/media/video/s5p-fimc/fimc-reg.c
@@ -13,7 +13,7 @@
 #include <linux/io.h>
 #include <linux/delay.h>
 #include <mach/map.h>
-#include <media/s3c_fimc.h>
+#include <media/s5p_fimc.h>
 
 #include "fimc-core.h"
 
@@ -37,11 +37,11 @@ void fimc_hw_reset(struct fimc_dev *dev)
 	writel(cfg, dev->regs + S5P_CIGCTRL);
 }
 
-static u32 fimc_hw_get_in_flip(u32 ctx_flip)
+static u32 fimc_hw_get_in_flip(struct fimc_ctx *ctx)
 {
 	u32 flip = S5P_MSCTRL_FLIP_NORMAL;
 
-	switch (ctx_flip) {
+	switch (ctx->flip) {
 	case FLIP_X_AXIS:
 		flip = S5P_MSCTRL_FLIP_X_MIRROR;
 		break;
@@ -51,16 +51,20 @@ static u32 fimc_hw_get_in_flip(u32 ctx_flip)
 	case FLIP_XY_AXIS:
 		flip = S5P_MSCTRL_FLIP_180;
 		break;
+	default:
+		break;
 	}
+	if (ctx->rotation <= 90)
+		return flip;
 
-	return flip;
+	return (flip ^ S5P_MSCTRL_FLIP_180) & S5P_MSCTRL_FLIP_180;
 }
 
-static u32 fimc_hw_get_target_flip(u32 ctx_flip)
+static u32 fimc_hw_get_target_flip(struct fimc_ctx *ctx)
 {
 	u32 flip = S5P_CITRGFMT_FLIP_NORMAL;
 
-	switch (ctx_flip) {
+	switch (ctx->flip) {
 	case FLIP_X_AXIS:
 		flip = S5P_CITRGFMT_FLIP_X_MIRROR;
 		break;
@@ -70,11 +74,13 @@ static u32 fimc_hw_get_target_flip(u32 ctx_flip)
 	case FLIP_XY_AXIS:
 		flip = S5P_CITRGFMT_FLIP_180;
 		break;
-	case FLIP_NONE:
+	default:
 		break;
-
 	}
-	return flip;
+	if (ctx->rotation <= 90)
+		return flip;
+
+	return (flip ^ S5P_CITRGFMT_FLIP_180) & S5P_CITRGFMT_FLIP_180;
 }
 
 void fimc_hw_set_rotation(struct fimc_ctx *ctx)
@@ -84,10 +90,7 @@ void fimc_hw_set_rotation(struct fimc_ctx *ctx)
 
 	cfg = readl(dev->regs + S5P_CITRGFMT);
 	cfg &= ~(S5P_CITRGFMT_INROT90 | S5P_CITRGFMT_OUTROT90 |
-		  S5P_CITRGFMT_FLIP_180);
-
-	flip = readl(dev->regs + S5P_MSCTRL);
-	flip &= ~S5P_MSCTRL_FLIP_MASK;
+		 S5P_CITRGFMT_FLIP_180);
 
 	/*
 	 * The input and output rotator cannot work simultaneously.
@@ -95,26 +98,22 @@ void fimc_hw_set_rotation(struct fimc_ctx *ctx)
 	 * in direct fifo output mode.
 	 */
 	if (ctx->rotation == 90 || ctx->rotation == 270) {
-		if (ctx->out_path == FIMC_LCDFIFO) {
-			cfg |= S5P_CITRGFMT_INROT90;
-			if (ctx->rotation == 270)
-				flip |= S5P_MSCTRL_FLIP_180;
-		} else {
-			cfg |= S5P_CITRGFMT_OUTROT90;
-			if (ctx->rotation == 270)
-				cfg |= S5P_CITRGFMT_FLIP_180;
-		}
-	} else if (ctx->rotation == 180) {
 		if (ctx->out_path == FIMC_LCDFIFO)
-			flip |= S5P_MSCTRL_FLIP_180;
+			cfg |= S5P_CITRGFMT_INROT90;
 		else
-			cfg |= S5P_CITRGFMT_FLIP_180;
+			cfg |= S5P_CITRGFMT_OUTROT90;
 	}
-	if (ctx->rotation == 180 || ctx->rotation == 270)
-		writel(flip, dev->regs + S5P_MSCTRL);
 
-	cfg |= fimc_hw_get_target_flip(ctx->flip);
-	writel(cfg, dev->regs + S5P_CITRGFMT);
+	if (ctx->out_path == FIMC_DMA) {
+		cfg |= fimc_hw_get_target_flip(ctx);
+		writel(cfg, dev->regs + S5P_CITRGFMT);
+	} else {
+		/* LCD FIFO path */
+		flip = readl(dev->regs + S5P_MSCTRL);
+		flip &= ~S5P_MSCTRL_FLIP_MASK;
+		flip |= fimc_hw_get_in_flip(ctx);
+		writel(flip, dev->regs + S5P_MSCTRL);
+	}
 }
 
 void fimc_hw_set_target_format(struct fimc_ctx *ctx)
@@ -131,19 +130,14 @@ void fimc_hw_set_target_format(struct fimc_ctx *ctx)
 		  S5P_CITRGFMT_VSIZE_MASK);
 
 	switch (frame->fmt->color) {
-	case S5P_FIMC_RGB565:
-	case S5P_FIMC_RGB666:
-	case S5P_FIMC_RGB888:
+	case S5P_FIMC_RGB565...S5P_FIMC_RGB888:
 		cfg |= S5P_CITRGFMT_RGB;
 		break;
 	case S5P_FIMC_YCBCR420:
 		cfg |= S5P_CITRGFMT_YCBCR420;
 		break;
-	case S5P_FIMC_YCBYCR422:
-	case S5P_FIMC_YCRYCB422:
-	case S5P_FIMC_CBYCRY422:
-	case S5P_FIMC_CRYCBY422:
-		if (frame->fmt->planes_cnt == 1)
+	case S5P_FIMC_YCBYCR422...S5P_FIMC_CRYCBY422:
+		if (frame->fmt->colplanes == 1)
 			cfg |= S5P_CITRGFMT_YCBCR422_1P;
 		else
 			cfg |= S5P_CITRGFMT_YCBCR422;
@@ -219,11 +213,11 @@ void fimc_hw_set_out_dma(struct fimc_ctx *ctx)
 	cfg &= ~(S5P_CIOCTRL_ORDER2P_MASK | S5P_CIOCTRL_ORDER422_MASK |
 		 S5P_CIOCTRL_YCBCR_PLANE_MASK);
 
-	if (frame->fmt->planes_cnt == 1)
+	if (frame->fmt->colplanes == 1)
 		cfg |= ctx->out_order_1p;
-	else if (frame->fmt->planes_cnt == 2)
+	else if (frame->fmt->colplanes == 2)
 		cfg |= ctx->out_order_2p | S5P_CIOCTRL_YCBCR_2PLANE;
-	else if (frame->fmt->planes_cnt == 3)
+	else if (frame->fmt->colplanes == 3)
 		cfg |= S5P_CIOCTRL_YCBCR_3PLANE;
 
 	writel(cfg, dev->regs + S5P_CIOCTRL);
@@ -249,7 +243,7 @@ void fimc_hw_en_lastirq(struct fimc_dev *dev, int enable)
 	writel(cfg, dev->regs + S5P_CIOCTRL);
 }
 
-static void fimc_hw_set_prescaler(struct fimc_ctx *ctx)
+void fimc_hw_set_prescaler(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *dev =  ctx->fimc_dev;
 	struct fimc_scaler *sc = &ctx->scaler;
@@ -267,7 +261,7 @@ static void fimc_hw_set_prescaler(struct fimc_ctx *ctx)
 	writel(cfg, dev->regs + S5P_CISCPREDST);
 }
 
-void fimc_hw_set_scaler(struct fimc_ctx *ctx)
+static void fimc_hw_set_scaler(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *dev = ctx->fimc_dev;
 	struct fimc_scaler *sc = &ctx->scaler;
@@ -275,8 +269,6 @@ void fimc_hw_set_scaler(struct fimc_ctx *ctx)
 	struct fimc_frame *dst_frame = &ctx->d_frame;
 	u32 cfg = 0;
 
-	fimc_hw_set_prescaler(ctx);
-
 	if (!(ctx->flags & FIMC_COLOR_RANGE_NARROW))
 		cfg |= (S5P_CISCCTRL_CSCR2Y_WIDE | S5P_CISCCTRL_CSCY2R_WIDE);
 
@@ -316,13 +308,42 @@ void fimc_hw_set_scaler(struct fimc_ctx *ctx)
 			cfg |= S5P_CISCCTRL_INTERLACE;
 	}
 
+	writel(cfg, dev->regs + S5P_CISCCTRL);
+}
+
+void fimc_hw_set_mainscaler(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *dev = ctx->fimc_dev;
+	struct samsung_fimc_variant *variant = dev->variant;
+	struct fimc_scaler *sc = &ctx->scaler;
+	u32 cfg;
+
 	dbg("main_hratio= 0x%X  main_vratio= 0x%X",
 		sc->main_hratio, sc->main_vratio);
 
-	cfg |= S5P_CISCCTRL_SC_HORRATIO(sc->main_hratio);
-	cfg |= S5P_CISCCTRL_SC_VERRATIO(sc->main_vratio);
+	fimc_hw_set_scaler(ctx);
 
-	writel(cfg, dev->regs + S5P_CISCCTRL);
+	cfg = readl(dev->regs + S5P_CISCCTRL);
+
+	if (variant->has_mainscaler_ext) {
+		cfg &= ~(S5P_CISCCTRL_MHRATIO_MASK | S5P_CISCCTRL_MVRATIO_MASK);
+		cfg |= S5P_CISCCTRL_MHRATIO_EXT(sc->main_hratio);
+		cfg |= S5P_CISCCTRL_MVRATIO_EXT(sc->main_vratio);
+		writel(cfg, dev->regs + S5P_CISCCTRL);
+
+		cfg = readl(dev->regs + S5P_CIEXTEN);
+
+		cfg &= ~(S5P_CIEXTEN_MVRATIO_EXT_MASK |
+			 S5P_CIEXTEN_MHRATIO_EXT_MASK);
+		cfg |= S5P_CIEXTEN_MHRATIO_EXT(sc->main_hratio);
+		cfg |= S5P_CIEXTEN_MVRATIO_EXT(sc->main_vratio);
+		writel(cfg, dev->regs + S5P_CIEXTEN);
+	} else {
+		cfg &= ~(S5P_CISCCTRL_MHRATIO_MASK | S5P_CISCCTRL_MVRATIO_MASK);
+		cfg |= S5P_CISCCTRL_MHRATIO(sc->main_hratio);
+		cfg |= S5P_CISCCTRL_MVRATIO(sc->main_vratio);
+		writel(cfg, dev->regs + S5P_CISCCTRL);
+	}
 }
 
 void fimc_hw_en_capture(struct fimc_ctx *ctx)
@@ -335,7 +356,7 @@ void fimc_hw_en_capture(struct fimc_ctx *ctx)
 		/* one shot mode */
 		cfg |= S5P_CIIMGCPT_CPT_FREN_ENABLE | S5P_CIIMGCPT_IMGCPTEN;
 	} else {
-		/* Continous frame capture mode (freerun). */
+		/* Continuous frame capture mode (freerun). */
 		cfg &= ~(S5P_CIIMGCPT_CPT_FREN_ENABLE |
 			 S5P_CIIMGCPT_CPT_FRMOD_CNT);
 		cfg |= S5P_CIIMGCPT_IMGCPTEN;
@@ -410,41 +431,37 @@ void fimc_hw_set_in_dma(struct fimc_ctx *ctx)
 
 	/* Set the input DMA to process single frame only. */
 	cfg = readl(dev->regs + S5P_MSCTRL);
-	cfg &= ~(S5P_MSCTRL_FLIP_MASK
-		| S5P_MSCTRL_INFORMAT_MASK
+	cfg &= ~(S5P_MSCTRL_INFORMAT_MASK
 		| S5P_MSCTRL_IN_BURST_COUNT_MASK
 		| S5P_MSCTRL_INPUT_MASK
 		| S5P_MSCTRL_C_INT_IN_MASK
 		| S5P_MSCTRL_2P_IN_ORDER_MASK);
 
-	cfg |= (S5P_MSCTRL_FRAME_COUNT(1) | S5P_MSCTRL_INPUT_MEMORY);
+	cfg |= (S5P_MSCTRL_IN_BURST_COUNT(4)
+		| S5P_MSCTRL_INPUT_MEMORY
+		| S5P_MSCTRL_FIFO_CTRL_FULL);
 
 	switch (frame->fmt->color) {
-	case S5P_FIMC_RGB565:
-	case S5P_FIMC_RGB666:
-	case S5P_FIMC_RGB888:
+	case S5P_FIMC_RGB565...S5P_FIMC_RGB888:
 		cfg |= S5P_MSCTRL_INFORMAT_RGB;
 		break;
 	case S5P_FIMC_YCBCR420:
 		cfg |= S5P_MSCTRL_INFORMAT_YCBCR420;
 
-		if (frame->fmt->planes_cnt == 2)
+		if (frame->fmt->colplanes == 2)
 			cfg |= ctx->in_order_2p | S5P_MSCTRL_C_INT_IN_2PLANE;
 		else
 			cfg |= S5P_MSCTRL_C_INT_IN_3PLANE;
 
 		break;
-	case S5P_FIMC_YCBYCR422:
-	case S5P_FIMC_YCRYCB422:
-	case S5P_FIMC_CBYCRY422:
-	case S5P_FIMC_CRYCBY422:
-		if (frame->fmt->planes_cnt == 1) {
+	case S5P_FIMC_YCBYCR422...S5P_FIMC_CRYCBY422:
+		if (frame->fmt->colplanes == 1) {
 			cfg |= ctx->in_order_1p
 				| S5P_MSCTRL_INFORMAT_YCBCR422_1P;
 		} else {
 			cfg |= S5P_MSCTRL_INFORMAT_YCBCR422;
 
-			if (frame->fmt->planes_cnt == 2)
+			if (frame->fmt->colplanes == 2)
 				cfg |= ctx->in_order_2p
 					| S5P_MSCTRL_C_INT_IN_2PLANE;
 			else
@@ -455,13 +472,6 @@ void fimc_hw_set_in_dma(struct fimc_ctx *ctx)
 		break;
 	}
 
-	/*
-	 * Input DMA flip mode (and rotation).
-	 * Do not allow simultaneous rotation and flipping.
-	 */
-	if (!ctx->rotation && ctx->out_path == FIMC_LCDFIFO)
-		cfg |= fimc_hw_get_in_flip(ctx->flip);
-
 	writel(cfg, dev->regs + S5P_MSCTRL);
 
 	/* Input/output DMA linear/tiled mode. */
@@ -532,7 +542,7 @@ void fimc_hw_set_output_addr(struct fimc_dev *dev,
 }
 
 int fimc_hw_set_camera_polarity(struct fimc_dev *fimc,
-				struct s3c_fimc_isp_info *cam)
+				struct s5p_fimc_isp_info *cam)
 {
 	u32 cfg = readl(fimc->regs + S5P_CIGCTRL);
 
@@ -557,41 +567,46 @@ int fimc_hw_set_camera_polarity(struct fimc_dev *fimc,
 }
 
 int fimc_hw_set_camera_source(struct fimc_dev *fimc,
-			      struct s3c_fimc_isp_info *cam)
+			      struct s5p_fimc_isp_info *cam)
 {
 	struct fimc_frame *f = &fimc->vid_cap.ctx->s_frame;
 	u32 cfg = 0;
+	u32 bus_width;
+	int i;
+
+	static const struct {
+		u32 pixelcode;
+		u32 cisrcfmt;
+		u16 bus_width;
+	} pix_desc[] = {
+		{ V4L2_MBUS_FMT_YUYV8_2X8, S5P_CISRCFMT_ORDER422_YCBYCR, 8 },
+		{ V4L2_MBUS_FMT_YVYU8_2X8, S5P_CISRCFMT_ORDER422_YCRYCB, 8 },
+		{ V4L2_MBUS_FMT_VYUY8_2X8, S5P_CISRCFMT_ORDER422_CRYCBY, 8 },
+		{ V4L2_MBUS_FMT_UYVY8_2X8, S5P_CISRCFMT_ORDER422_CBYCRY, 8 },
+		/* TODO: Add pixel codes for 16-bit bus width */
+	};
 
 	if (cam->bus_type == FIMC_ITU_601 || cam->bus_type == FIMC_ITU_656) {
+		for (i = 0; i < ARRAY_SIZE(pix_desc); i++) {
+			if (fimc->vid_cap.fmt.code == pix_desc[i].pixelcode) {
+				cfg = pix_desc[i].cisrcfmt;
+				bus_width = pix_desc[i].bus_width;
+				break;
+			}
+		}
 
-		switch (fimc->vid_cap.fmt.code) {
-		case V4L2_MBUS_FMT_YUYV8_2X8:
-			cfg = S5P_CISRCFMT_ORDER422_YCBYCR;
-			break;
-		case V4L2_MBUS_FMT_YVYU8_2X8:
-			cfg = S5P_CISRCFMT_ORDER422_YCRYCB;
-			break;
-		case V4L2_MBUS_FMT_VYUY8_2X8:
-			cfg = S5P_CISRCFMT_ORDER422_CRYCBY;
-			break;
-		case V4L2_MBUS_FMT_UYVY8_2X8:
-			cfg = S5P_CISRCFMT_ORDER422_CBYCRY;
-			break;
-		default:
-			err("camera image format not supported: %d",
-			    fimc->vid_cap.fmt.code);
+		if (i == ARRAY_SIZE(pix_desc)) {
+			v4l2_err(&fimc->vid_cap.v4l2_dev,
+				 "Camera color format not supported: %d\n",
+				 fimc->vid_cap.fmt.code);
 			return -EINVAL;
 		}
 
 		if (cam->bus_type == FIMC_ITU_601) {
-			if (cam->bus_width == 8) {
+			if (bus_width == 8)
 				cfg |= S5P_CISRCFMT_ITU601_8BIT;
-			} else if (cam->bus_width == 16) {
+			else if (bus_width == 16)
 				cfg |= S5P_CISRCFMT_ITU601_16BIT;
-			} else {
-				err("invalid bus width: %d", cam->bus_width);
-				return -EINVAL;
-			}
 		} /* else defaults to ITU-R BT.656 8-bit */
 	}
 
@@ -624,7 +639,7 @@ int fimc_hw_set_camera_offset(struct fimc_dev *fimc, struct fimc_frame *f)
 }
 
 int fimc_hw_set_camera_type(struct fimc_dev *fimc,
-			    struct s3c_fimc_isp_info *cam)
+			    struct s5p_fimc_isp_info *cam)
 {
 	u32 cfg, tmp;
 	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
@@ -650,10 +665,12 @@ int fimc_hw_set_camera_type(struct fimc_dev *fimc,
 			    vid_cap->fmt.code);
 			return -EINVAL;
 		}
-		writel(tmp | (0x1 << 8), fimc->regs + S5P_CSIIMGFMT);
+		tmp |= (cam->csi_data_align == 32) << 8;
+
+		writel(tmp, fimc->regs + S5P_CSIIMGFMT);
 
 	} else if (cam->bus_type == FIMC_ITU_601 ||
-		  cam->bus_type == FIMC_ITU_656) {
+		   cam->bus_type == FIMC_ITU_656) {
 		if (cam->mux_id == 0) /* ITU-A, ITU-B: 0, 1 */
 			cfg |= S5P_CIGCTRL_SELCAM_ITU_A;
 	} else if (cam->bus_type == FIMC_LCD_WB) {
diff --git a/drivers/media/video/s5p-fimc/regs-fimc.h b/drivers/media/video/s5p-fimc/regs-fimc.h
index 57e33f84fcfa..0fea3e635d76 100644
--- a/drivers/media/video/s5p-fimc/regs-fimc.h
+++ b/drivers/media/video/s5p-fimc/regs-fimc.h
@@ -98,8 +98,8 @@
 #define S5P_CIOCTRL			0x4c
 #define S5P_CIOCTRL_ORDER422_MASK	(3 << 0)
 #define S5P_CIOCTRL_ORDER422_CRYCBY	(0 << 0)
-#define S5P_CIOCTRL_ORDER422_YCRYCB	(1 << 0)
-#define S5P_CIOCTRL_ORDER422_CBYCRY	(2 << 0)
+#define S5P_CIOCTRL_ORDER422_CBYCRY	(1 << 0)
+#define S5P_CIOCTRL_ORDER422_YCRYCB	(2 << 0)
 #define S5P_CIOCTRL_ORDER422_YCBYCR	(3 << 0)
 #define S5P_CIOCTRL_LASTIRQ_ENABLE	(1 << 2)
 #define S5P_CIOCTRL_YCBCR_3PLANE	(0 << 3)
@@ -139,8 +139,12 @@
 #define S5P_CISCCTRL_OUTRGB_FMT_MASK	(3 << 11)
 #define S5P_CISCCTRL_RGB_EXT		(1 << 10)
 #define S5P_CISCCTRL_ONE2ONE		(1 << 9)
-#define S5P_CISCCTRL_SC_HORRATIO(x)	((x) << 16)
-#define S5P_CISCCTRL_SC_VERRATIO(x)	((x) << 0)
+#define S5P_CISCCTRL_MHRATIO(x)		((x) << 16)
+#define S5P_CISCCTRL_MVRATIO(x)		((x) << 0)
+#define S5P_CISCCTRL_MHRATIO_MASK	(0x1ff << 16)
+#define S5P_CISCCTRL_MVRATIO_MASK	(0x1ff << 0)
+#define S5P_CISCCTRL_MHRATIO_EXT(x)	(((x) >> 6) << 16)
+#define S5P_CISCCTRL_MVRATIO_EXT(x)	(((x) >> 6) << 0)
 
 /* Target area */
 #define S5P_CITAREA			0x5c
@@ -210,7 +214,7 @@
 
 /* Input DMA control */
 #define S5P_MSCTRL			0xfc
-#define S5P_MSCTRL_IN_BURST_COUNT_MASK	(3 << 24)
+#define S5P_MSCTRL_IN_BURST_COUNT_MASK	(0xF << 24)
 #define S5P_MSCTRL_2P_IN_ORDER_MASK	(3 << 16)
 #define S5P_MSCTRL_2P_IN_ORDER_SHIFT	16
 #define S5P_MSCTRL_C_INT_IN_3PLANE	(0 << 15)
@@ -222,11 +226,12 @@
 #define S5P_MSCTRL_FLIP_X_MIRROR	(1 << 13)
 #define S5P_MSCTRL_FLIP_Y_MIRROR	(2 << 13)
 #define S5P_MSCTRL_FLIP_180		(3 << 13)
+#define S5P_MSCTRL_FIFO_CTRL_FULL	(1 << 12)
 #define S5P_MSCTRL_ORDER422_SHIFT	4
-#define S5P_MSCTRL_ORDER422_CRYCBY	(0 << 4)
-#define S5P_MSCTRL_ORDER422_YCRYCB	(1 << 4)
-#define S5P_MSCTRL_ORDER422_CBYCRY	(2 << 4)
-#define S5P_MSCTRL_ORDER422_YCBYCR	(3 << 4)
+#define S5P_MSCTRL_ORDER422_YCBYCR	(0 << 4)
+#define S5P_MSCTRL_ORDER422_CBYCRY	(1 << 4)
+#define S5P_MSCTRL_ORDER422_YCRYCB	(2 << 4)
+#define S5P_MSCTRL_ORDER422_CRYCBY	(3 << 4)
 #define S5P_MSCTRL_ORDER422_MASK	(3 << 4)
 #define S5P_MSCTRL_INPUT_EXTCAM		(0 << 3)
 #define S5P_MSCTRL_INPUT_MEMORY		(1 << 3)
@@ -237,7 +242,7 @@
 #define S5P_MSCTRL_INFORMAT_RGB		(3 << 1)
 #define S5P_MSCTRL_INFORMAT_MASK	(3 << 1)
 #define S5P_MSCTRL_ENVID		(1 << 0)
-#define S5P_MSCTRL_FRAME_COUNT(x)	((x) << 24)
+#define S5P_MSCTRL_IN_BURST_COUNT(x)	((x) << 24)
 
 /* Output DMA Y/Cb/Cr offset */
 #define S5P_CIOYOFF			0x168
@@ -263,6 +268,10 @@
 
 /* Real output DMA image size (extension register) */
 #define S5P_CIEXTEN			0x188
+#define S5P_CIEXTEN_MHRATIO_EXT(x)	(((x) & 0x3f) << 10)
+#define S5P_CIEXTEN_MVRATIO_EXT(x)	((x) & 0x3f)
+#define S5P_CIEXTEN_MHRATIO_EXT_MASK	(0x3f << 10)
+#define S5P_CIEXTEN_MVRATIO_EXT_MASK	0x3f
 
 #define S5P_CIDMAPARAM			0x18c
 #define S5P_CIDMAPARAM_R_LINEAR		(0 << 29)