msm: mdss: support rotator sessions for high resolution surfaces

Each rotator block only supports up to 2048 width max, in order to
rotate bigger surfaces they need to be split and rotated in two separate
rotator blocks. Add support to manage this split internally and abstract
it to user.

Change-Id: Ib870bad5683aca7863e51043c2b7f487810e3eb1
Signed-off-by: Adrian Salido-Moreno <adrianm@codeaurora.org>

8 files changed, 228 insertions, 94 deletions

diff --git a/drivers/video/fbdev/msm/mdss_mdp.h b/drivers/video/fbdev/msm/mdss_mdp.h
index a70720df3950..175a07f10959 100644
--- a/drivers/video/fbdev/msm/mdss_mdp.h
+++ b/drivers/video/fbdev/msm/mdss_mdp.h
@@ -468,6 +468,8 @@ int mdss_mdp_get_plane_sizes(u32 format, u32 w, u32 h,
 			     struct mdss_mdp_plane_sizes *ps, u32 bwc_mode);
 int mdss_mdp_get_rau_strides(u32 w, u32 h, struct mdss_mdp_format_params *fmt,
 			       struct mdss_mdp_plane_sizes *ps);
+void mdss_mdp_data_calc_offset(struct mdss_mdp_data *data, u16 x, u16 y,
+	struct mdss_mdp_plane_sizes *ps, struct mdss_mdp_format_params *fmt);
 struct mdss_mdp_format_params *mdss_mdp_get_format_params(u32 format);
 int mdss_mdp_put_img(struct mdss_mdp_img_data *data);
 int mdss_mdp_get_img(struct msmfb_data *img, struct mdss_mdp_img_data *data);
diff --git a/drivers/video/fbdev/msm/mdss_mdp_ctl.c b/drivers/video/fbdev/msm/mdss_mdp_ctl.c
index 4e51100c0fe8..03a33cd943fc 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_ctl.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_ctl.c
@@ -279,8 +279,6 @@ static int mdss_mdp_ctl_free(struct mdss_mdp_ctl *ctl)
 		return -EINVAL;
 	}
 
-	mutex_lock(&mdss_mdp_ctl_lock);
-	ctl->ref_cnt--;
 	if (ctl->mixer_left) {
 		mdss_mdp_mixer_free(ctl->mixer_left);
 		ctl->mixer_left = NULL;
@@ -289,6 +287,8 @@ static int mdss_mdp_ctl_free(struct mdss_mdp_ctl *ctl)
 		mdss_mdp_mixer_free(ctl->mixer_right);
 		ctl->mixer_right = NULL;
 	}
+	mutex_lock(&mdss_mdp_ctl_lock);
+	ctl->ref_cnt--;
 	ctl->power_on = false;
 	ctl->start_fnc = NULL;
 	ctl->stop_fnc = NULL;
@@ -437,7 +437,6 @@ int mdss_mdp_wb_mixer_destroy(struct mdss_mdp_mixer *mixer)
 	if (ctl->stop_fnc)
 		ctl->stop_fnc(ctl);
 
-	mdss_mdp_mixer_free(mixer);
 	mdss_mdp_ctl_free(ctl);
 
 	mdss_mdp_ctl_perf_commit(ctl->mdata, MDSS_MDP_PERF_UPDATE_ALL);
diff --git a/drivers/video/fbdev/msm/mdss_mdp_intf_writeback.c b/drivers/video/fbdev/msm/mdss_mdp_intf_writeback.c
index 7fbb0312dec1..b3f15a7148cd 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_intf_writeback.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_intf_writeback.c
@@ -33,9 +33,11 @@ struct mdss_mdp_writeback_ctx {
 	u32 intf_num;
 
 	u32 opmode;
-	u32 format;
+	struct mdss_mdp_format_params *dst_fmt;
 	u16 width;
 	u16 height;
+	struct mdss_mdp_img_rect dst_rect;
+
 	u8 rot90;
 	u32 bwc_mode;
 	int initialized;
@@ -81,48 +83,55 @@ static inline void mdp_wb_write(struct mdss_mdp_writeback_ctx *ctx,
 }
 
 static int mdss_mdp_writeback_addr_setup(struct mdss_mdp_writeback_ctx *ctx,
-					 struct mdss_mdp_data *data)
+					 const struct mdss_mdp_data *in_data)
 {
 	int ret;
+	struct mdss_mdp_data data;
 
-	if (!data)
+	if (!in_data)
 		return -EINVAL;
+	data = *in_data;
 
-	pr_debug("wb_num=%d addr=0x%x\n", ctx->wb_num, data->p[0].addr);
+	pr_debug("wb_num=%d addr=0x%x\n", ctx->wb_num, data.p[0].addr);
 
 	if (ctx->bwc_mode)
-		data->bwc_enabled = 1;
+		data.bwc_enabled = 1;
 
-	ret = mdss_mdp_data_check(data, &ctx->dst_planes);
+	ret = mdss_mdp_data_check(&data, &ctx->dst_planes);
 	if (ret)
 		return ret;
 
-	mdp_wb_write(ctx, MDSS_MDP_REG_WB_DST0_ADDR, data->p[0].addr);
-	mdp_wb_write(ctx, MDSS_MDP_REG_WB_DST1_ADDR, data->p[1].addr);
-	mdp_wb_write(ctx, MDSS_MDP_REG_WB_DST2_ADDR, data->p[2].addr);
-	mdp_wb_write(ctx, MDSS_MDP_REG_WB_DST3_ADDR, data->p[3].addr);
+	mdss_mdp_data_calc_offset(&data, ctx->dst_rect.x, ctx->dst_rect.y,
+			&ctx->dst_planes, ctx->dst_fmt);
+
+	mdp_wb_write(ctx, MDSS_MDP_REG_WB_DST0_ADDR, data.p[0].addr);
+	mdp_wb_write(ctx, MDSS_MDP_REG_WB_DST1_ADDR, data.p[1].addr);
+	mdp_wb_write(ctx, MDSS_MDP_REG_WB_DST2_ADDR, data.p[2].addr);
+	mdp_wb_write(ctx, MDSS_MDP_REG_WB_DST3_ADDR, data.p[3].addr);
 
 	return 0;
 }
 
-static int mdss_mdp_writeback_format_setup(struct mdss_mdp_writeback_ctx *ctx)
+static int mdss_mdp_writeback_format_setup(struct mdss_mdp_writeback_ctx *ctx,
+		u32 format)
 {
 	struct mdss_mdp_format_params *fmt;
 	u32 dst_format, pattern, ystride0, ystride1, outsize, chroma_samp;
 	u32 opmode = ctx->opmode;
 	struct mdss_data_type *mdata;
 
-	pr_debug("wb_num=%d format=%d\n", ctx->wb_num, ctx->format);
+	pr_debug("wb_num=%d format=%d\n", ctx->wb_num, format);
 
-	mdss_mdp_get_plane_sizes(ctx->format, ctx->width, ctx->height,
+	mdss_mdp_get_plane_sizes(format, ctx->width, ctx->height,
 				 &ctx->dst_planes,
 				 ctx->opmode & MDSS_MDP_OP_BWC_EN);
 
-	fmt = mdss_mdp_get_format_params(ctx->format);
+	fmt = mdss_mdp_get_format_params(format);
 	if (!fmt) {
-		pr_err("wb format=%d not supported\n", ctx->format);
+		pr_err("wb format=%d not supported\n", format);
 		return -EINVAL;
 	}
+	ctx->dst_fmt = fmt;
 
 	chroma_samp = fmt->chroma_sample;
 
@@ -190,7 +199,7 @@ static int mdss_mdp_writeback_format_setup(struct mdss_mdp_writeback_ctx *ctx)
 		   (ctx->dst_planes.ystride[1] << 16);
 	ystride1 = (ctx->dst_planes.ystride[2]) |
 		   (ctx->dst_planes.ystride[3] << 16);
-	outsize = (ctx->height << 16) | ctx->width;
+	outsize = (ctx->dst_rect.h << 16) | ctx->dst_rect.w;
 
 	mdp_wb_write(ctx, MDSS_MDP_REG_WB_DST_FORMAT, dst_format);
 	mdp_wb_write(ctx, MDSS_MDP_REG_WB_DST_OP_MODE, opmode);
@@ -217,11 +226,14 @@ static int mdss_mdp_writeback_prepare_wfd(struct mdss_mdp_ctl *ctl, void *arg)
 	pr_debug("wfd setup ctl=%d\n", ctl->num);
 
 	ctx->opmode = 0;
-	ctx->format = ctl->dst_format;
 	ctx->width = ctl->width;
 	ctx->height = ctl->height;
+	ctx->dst_rect.x = 0;
+	ctx->dst_rect.y = 0;
+	ctx->dst_rect.w = ctx->width;
+	ctx->dst_rect.h = ctx->height;
 
-	ret = mdss_mdp_writeback_format_setup(ctx);
+	ret = mdss_mdp_writeback_format_setup(ctx, ctl->dst_format);
 	if (ret) {
 		pr_err("format setup failed\n");
 		return ret;
@@ -237,6 +249,7 @@ static int mdss_mdp_writeback_prepare_rot(struct mdss_mdp_ctl *ctl, void *arg)
 	struct mdss_mdp_writeback_ctx *ctx;
 	struct mdss_mdp_writeback_arg *wb_args;
 	struct mdss_mdp_rotator_session *rot;
+	u32 format;
 
 	ctx = (struct mdss_mdp_writeback_ctx *) ctl->priv_data;
 	if (!ctx)
@@ -259,24 +272,26 @@ static int mdss_mdp_writeback_prepare_rot(struct mdss_mdp_ctl *ctl, void *arg)
 	ctx->bwc_mode = rot->bwc_mode;
 	ctx->opmode |= ctx->bwc_mode;
 
-	ctx->width = rot->src_rect.w;
-	ctx->height = rot->src_rect.h;
-
-	ctx->format = rot->format;
+	ctx->width = rot->dst.w;
+	ctx->height = rot->dst.h;
+	ctx->dst_rect.x = rot->dst.x;
+	ctx->dst_rect.y = rot->dst.y;
+	ctx->dst_rect.w = rot->src_rect.w;
+	ctx->dst_rect.h = rot->src_rect.h;
 
 	ctx->rot90 = !!(rot->flags & MDP_ROT_90);
 
 	if (ctx->bwc_mode || ctx->rot90)
-		ctx->format = mdss_mdp_get_rotator_dst_format(rot->format);
+		format = mdss_mdp_get_rotator_dst_format(rot->format);
 	else
-		ctx->format = rot->format;
+		format = rot->format;
 
 	if (ctx->rot90) {
 		ctx->opmode |= BIT(5); /* ROT 90 */
-		swap(ctx->width, ctx->height);
+		swap(ctx->dst_rect.w, ctx->dst_rect.h);
 	}
 
-	return mdss_mdp_writeback_format_setup(ctx);
+	return mdss_mdp_writeback_format_setup(ctx, format);
 }
 
 static int mdss_mdp_writeback_stop(struct mdss_mdp_ctl *ctl)
diff --git a/drivers/video/fbdev/msm/mdss_mdp_overlay.c b/drivers/video/fbdev/msm/mdss_mdp_overlay.c
index a0ede3ca184f..3cce4f9c15d3 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_overlay.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_overlay.c
@@ -92,8 +92,7 @@ static int mdss_mdp_overlay_req_check(struct msm_fb_data_type *mfd,
 		return -EOVERFLOW;
 	}
 
-	if (req->dst_rect.w < min_dst_size || req->dst_rect.h < min_dst_size ||
-	    req->dst_rect.w > MAX_DST_W || req->dst_rect.h > MAX_DST_H) {
+	if (req->dst_rect.w < min_dst_size || req->dst_rect.h < min_dst_size) {
 		pr_err("invalid destination resolution (%dx%d)",
 		       req->dst_rect.w, req->dst_rect.h);
 		return -EOVERFLOW;
@@ -227,9 +226,13 @@ static int mdss_mdp_overlay_rotator_setup(struct msm_fb_data_type *mfd,
 		rot->src_rect.h /= 2;
 	}
 
-	rot->params_changed++;
-
-	req->id = rot->session_id;
+	ret = mdss_mdp_rotator_setup(rot);
+	if (ret == 0) {
+		req->id = rot->session_id;
+	} else {
+		pr_err("Unable to setup rotator session\n");
+		mdss_mdp_rotator_release(rot->session_id);
+	}
 
 	return ret;
 }
@@ -249,6 +252,17 @@ static int mdss_mdp_overlay_pipe_setup(struct msm_fb_data_type *mfd,
 	if (mdp5_data->ctl == NULL)
 		return -ENODEV;
 
+	if (req->flags & MDP_ROT_90) {
+		pr_err("unsupported inline rotation\n");
+		return -ENOTSUPP;
+	}
+
+	if ((req->dst_rect.w > MAX_DST_W) || (req->dst_rect.h > MAX_DST_H)) {
+		pr_err("exceeded max mixer supported resolution %dx%d\n",
+				req->dst_rect.w, req->dst_rect.h);
+		return -EOVERFLOW;
+	}
+
 	if (req->flags & MDSS_MDP_RIGHT_MIXER)
 		mixer_mux = MDSS_MDP_MIXER_MUX_RIGHT;
 	else
@@ -257,11 +271,6 @@ static int mdss_mdp_overlay_pipe_setup(struct msm_fb_data_type *mfd,
 	pr_debug("pipe ctl=%u req id=%x mux=%d\n", mdp5_data->ctl->num, req->id,
 			mixer_mux);
 
-	if (req->flags & MDP_ROT_90) {
-		pr_err("unsupported inline rotation\n");
-		return -ENOTSUPP;
-	}
-
 	src_format = req->src.format;
 	if (req->flags & (MDP_SOURCE_ROTATED_90 | MDP_BWC_EN))
 		src_format = mdss_mdp_get_rotator_dst_format(src_format);
@@ -366,7 +375,8 @@ static int mdss_mdp_overlay_pipe_setup(struct msm_fb_data_type *mfd,
 	pipe->is_fg = req->is_fg;
 	pipe->alpha = req->alpha;
 	pipe->transp = req->transp_mask;
-	pipe->overfetch_disable = fmt->is_yuv;
+	pipe->overfetch_disable = fmt->is_yuv &&
+			!(pipe->flags & MDP_SOURCE_ROTATED_90);
 
 	pipe->req_data = *req;
 
@@ -1603,10 +1613,8 @@ static int mdss_mdp_overlay_ioctl_handler(struct msm_fb_data_type *mfd,
 				ret = copy_to_user(argp, &req, sizeof(req));
 		}
 
-		if (ret) {
+		if (ret)
 			pr_debug("OVERLAY_GET failed (%d)\n", ret);
-			ret = -EFAULT;
-		}
 		break;
 
 	case MSMFB_OVERLAY_SET:
@@ -1617,10 +1625,8 @@ static int mdss_mdp_overlay_ioctl_handler(struct msm_fb_data_type *mfd,
 			if (!IS_ERR_VALUE(ret))
 				ret = copy_to_user(argp, &req, sizeof(req));
 		}
-		if (ret) {
+		if (ret)
 			pr_debug("OVERLAY_SET failed (%d)\n", ret);
-			ret = -EFAULT;
-		}
 		break;
 
 
@@ -1649,10 +1655,8 @@ static int mdss_mdp_overlay_ioctl_handler(struct msm_fb_data_type *mfd,
 					mdss_fb_update_backlight(mfd);
 			}
 
-			if (ret) {
+			if (ret)
 				pr_debug("OVERLAY_PLAY failed (%d)\n", ret);
-				ret = -EFAULT;
-			}
 		} else {
 			ret = 0;
 		}
@@ -1666,10 +1670,8 @@ static int mdss_mdp_overlay_ioctl_handler(struct msm_fb_data_type *mfd,
 			if (!ret)
 				ret = mdss_mdp_overlay_play_wait(mfd, &data);
 
-			if (ret) {
+			if (ret)
 				pr_err("OVERLAY_PLAY_WAIT failed (%d)\n", ret);
-				ret = -EFAULT;
-			}
 		} else {
 			ret = 0;
 		}
diff --git a/drivers/video/fbdev/msm/mdss_mdp_pipe.c b/drivers/video/fbdev/msm/mdss_mdp_pipe.c
index b8020c418b6d..242972bb5463 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_pipe.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_pipe.c
@@ -487,28 +487,6 @@ static int mdss_mdp_format_setup(struct mdss_mdp_pipe *pipe)
 	return 0;
 }
 
-static void mdss_mdp_addr_add_offset(struct mdss_mdp_pipe *pipe,
-				    struct mdss_mdp_data *data)
-{
-	data->p[0].addr += pipe->src.x +
-		(pipe->src.y * pipe->src_planes.ystride[0]);
-	if (data->num_planes > 1) {
-		u8 hmap[] = { 1, 2, 1, 2 };
-		u8 vmap[] = { 1, 1, 2, 2 };
-		u16 xoff = pipe->src.x / hmap[pipe->src_fmt->chroma_sample];
-		u16 yoff = pipe->src.y / vmap[pipe->src_fmt->chroma_sample];
-
-		if (data->num_planes == 2) /* pseudo planar */
-			xoff *= 2;
-		data->p[1].addr += xoff + (yoff * pipe->src_planes.ystride[1]);
-
-		if (data->num_planes > 2) { /* planar */
-			data->p[2].addr += xoff +
-				(yoff * pipe->src_planes.ystride[2]);
-		}
-	}
-}
-
 int mdss_mdp_pipe_addr_setup(struct mdss_data_type *mdata, u32 *offsets,
 				u32 *ftch_id, u32 type, u32 num_base, u32 len)
 {
@@ -571,7 +549,8 @@ static int mdss_mdp_src_addr_setup(struct mdss_mdp_pipe *pipe,
 		return ret;
 
 	if (pipe->overfetch_disable)
-		mdss_mdp_addr_add_offset(pipe, data);
+		mdss_mdp_data_calc_offset(data, pipe->src.x, pipe->src.y,
+			&pipe->src_planes, pipe->src_fmt);
 
 	/* planar format expects YCbCr, swap chroma planes if YCrCb */
 	if (!is_rot && (pipe->src_fmt->fetch_planes == MDSS_MDP_PLANE_PLANAR) &&
diff --git a/drivers/video/fbdev/msm/mdss_mdp_rotator.c b/drivers/video/fbdev/msm/mdss_mdp_rotator.c
index 5711653f8330..ce4c28fdf40a 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_rotator.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_rotator.c
@@ -27,6 +27,8 @@ static DEFINE_MUTEX(rotator_lock);
 static struct mdss_mdp_rotator_session rotator_session[MAX_ROTATOR_SESSIONS];
 static LIST_HEAD(rotator_queue);
 
+static int mdss_mdp_rotator_finish(struct mdss_mdp_rotator_session *rot);
+
 struct mdss_mdp_rotator_session *mdss_mdp_rotator_session_alloc(void)
 {
 	struct mdss_mdp_rotator_session *rot;
@@ -166,27 +168,21 @@ static int mdss_mdp_rotator_pipe_dequeue(struct mdss_mdp_rotator_session *rot)
 	return 0;
 }
 
-int mdss_mdp_rotator_queue(struct mdss_mdp_rotator_session *rot,
+static int mdss_mdp_rotator_queue_sub(struct mdss_mdp_rotator_session *rot,
 			   struct mdss_mdp_data *src_data,
 			   struct mdss_mdp_data *dst_data)
 {
 	struct mdss_mdp_pipe *rot_pipe = NULL;
 	struct mdss_mdp_ctl *ctl;
-	int ret, need_wait = false;
-
-	ret = mutex_lock_interruptible(&rotator_lock);
-	if (ret)
-		return ret;
+	int ret;
 
-	if (!rot || !rot->ref_cnt) {
-		mutex_unlock(&rotator_lock);
-		return -ENODEV;
-	}
+	if (!rot || !rot->ref_cnt)
+		return -ENOENT;
 
 	ret = mdss_mdp_rotator_pipe_dequeue(rot);
 	if (ret) {
 		pr_err("unable to acquire rotator\n");
-		goto done;
+		return ret;
 	}
 
 	rot_pipe = rot->pipe;
@@ -203,31 +199,141 @@ int mdss_mdp_rotator_queue(struct mdss_mdp_rotator_session *rot,
 		rot_pipe->img_height = rot->img_height;
 		rot_pipe->src = rot->src_rect;
 		rot_pipe->dst = rot->src_rect;
+		rot_pipe->dst.x = 0;
+		rot_pipe->dst.y = 0;
 		rot_pipe->params_changed++;
 	}
 
 	ret = mdss_mdp_pipe_queue_data(rot->pipe, src_data);
 	if (ret) {
 		pr_err("unable to queue rot data\n");
-		goto done;
+		return ret;
 	}
 
 	ret = mdss_mdp_rotator_kickoff(ctl, rot, dst_data);
 
-	if (ret == 0 && !rot->no_wait)
-		need_wait = true;
-done:
+	return ret;
+}
+
+int mdss_mdp_rotator_queue(struct mdss_mdp_rotator_session *rot,
+			   struct mdss_mdp_data *src_data,
+			   struct mdss_mdp_data *dst_data)
+{
+	int ret;
+	struct mdss_mdp_rotator_session *tmp = rot;
+
+	ret = mutex_lock_interruptible(&rotator_lock);
+	if (ret)
+		return ret;
+
+	pr_debug("rotator session=%x start\n", rot->session_id);
+
+	for (ret = 0, tmp = rot; ret == 0 && tmp; tmp = tmp->next)
+		ret = mdss_mdp_rotator_queue_sub(tmp, src_data, dst_data);
+
 	mutex_unlock(&rotator_lock);
 
-	if (need_wait)
-		mdss_mdp_rotator_busy_wait(rot);
+	if (ret) {
+		pr_err("rotation failed %d for rot=%d\n", ret, rot->session_id);
+		return ret;
+	}
+
+	for (tmp = rot; tmp; tmp = tmp->next)
+		mdss_mdp_rotator_busy_wait(tmp);
 
-	if (rot_pipe)
-		pr_debug("end of rotator pnum=%d enqueue\n", rot_pipe->num);
+	pr_debug("rotator session=%x queue done\n", rot->session_id);
 
 	return ret;
 }
 
+int mdss_mdp_rotator_setup(struct mdss_mdp_rotator_session *rot)
+{
+
+	rot->dst = rot->src_rect;
+	/*
+	 * by default, rotator output should be placed directly on
+	 * output buffer address without any offset.
+	 */
+	rot->dst.x = 0;
+	rot->dst.y = 0;
+
+	if (rot->flags & MDP_ROT_90)
+		swap(rot->dst.w, rot->dst.h);
+
+	if (rot->src_rect.w > MAX_MIXER_WIDTH) {
+		struct mdss_mdp_rotator_session *tmp;
+		u32 width;
+
+		if (rot->bwc_mode) {
+			pr_err("Unable to do split rotation with bwc set\n");
+			return -EINVAL;
+		}
+
+		width = rot->src_rect.w;
+
+		pr_debug("setting up split rotation src=%dx%d\n",
+			rot->src_rect.w, rot->src_rect.h);
+
+		if (width > (MAX_MIXER_WIDTH * 2)) {
+			pr_err("unsupported source width %d\n", width);
+			return -EOVERFLOW;
+		}
+
+		if (!rot->next) {
+			tmp = mdss_mdp_rotator_session_alloc();
+			if (!tmp) {
+				pr_err("unable to allocate rot dual session\n");
+				return -ENOMEM;
+			}
+			rot->next = tmp;
+		}
+		tmp = rot->next;
+
+		tmp->session_id = rot->session_id & ~MDSS_MDP_ROT_SESSION_MASK;
+		tmp->flags = rot->flags;
+		tmp->format = rot->format;
+		tmp->img_width = rot->img_width;
+		tmp->img_height = rot->img_height;
+		tmp->src_rect = rot->src_rect;
+
+		tmp->src_rect.w = width / 2;
+		width -= tmp->src_rect.w;
+		tmp->src_rect.x += width;
+
+		tmp->dst = rot->dst;
+		rot->src_rect.w = width;
+
+		if (rot->flags & MDP_ROT_90) {
+			/*
+			 * If rotated by 90 first half should be on top.
+			 * But if horizontally flipped should be on bottom.
+			 */
+			if (rot->flags & MDP_FLIP_LR)
+				rot->dst.y = tmp->src_rect.w;
+			else
+				tmp->dst.y = rot->src_rect.w;
+		} else {
+			/*
+			 * If not rotated, first half should be the left part
+			 * of the frame, unless horizontally flipped
+			 */
+			if (rot->flags & MDP_FLIP_LR)
+				rot->dst.x = tmp->src_rect.w;
+			else
+				tmp->dst.x = rot->src_rect.w;
+		}
+
+		tmp->params_changed++;
+	} else if (rot->next) {
+		mdss_mdp_rotator_finish(rot->next);
+		rot->next = NULL;
+	}
+
+	rot->params_changed++;
+
+	return 0;
+}
+
 static int mdss_mdp_rotator_finish(struct mdss_mdp_rotator_session *rot)
 {
 	struct mdss_mdp_pipe *rot_pipe;
@@ -237,6 +343,9 @@ static int mdss_mdp_rotator_finish(struct mdss_mdp_rotator_session *rot)
 
 	pr_debug("finish rot id=%x\n", rot->session_id);
 
+	if (rot->next)
+		mdss_mdp_rotator_finish(rot->next);
+
 	rot_pipe = rot->pipe;
 	if (rot_pipe) {
 		mdss_mdp_rotator_busy_wait(rot);
diff --git a/drivers/video/fbdev/msm/mdss_mdp_rotator.h b/drivers/video/fbdev/msm/mdss_mdp_rotator.h
index 21ee9bbfbbcb..c50d71085448 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_rotator.h
+++ b/drivers/video/fbdev/msm/mdss_mdp_rotator.h
@@ -30,6 +30,7 @@ struct mdss_mdp_rotator_session {
 	u16 img_width;
 	u16 img_height;
 	struct mdss_mdp_img_rect src_rect;
+	struct mdss_mdp_img_rect dst;
 
 	u32 bwc_mode;
 	struct mdss_mdp_pipe *pipe;
@@ -40,6 +41,7 @@ struct mdss_mdp_rotator_session {
 	u8 no_wait;
 
 	struct list_head head;
+	struct mdss_mdp_rotator_session *next;
 };
 
 static inline u32 mdss_mdp_get_rotator_dst_format(u32 in_format)
@@ -61,6 +63,7 @@ static inline u32 mdss_mdp_get_rotator_dst_format(u32 in_format)
 struct mdss_mdp_rotator_session *mdss_mdp_rotator_session_alloc(void);
 struct mdss_mdp_rotator_session *mdss_mdp_rotator_session_get(u32 session_id);
 
+int mdss_mdp_rotator_setup(struct mdss_mdp_rotator_session *rot);
 int mdss_mdp_rotator_queue(struct mdss_mdp_rotator_session *rot,
 			   struct mdss_mdp_data *src_data,
 			   struct mdss_mdp_data *dst_data);
diff --git a/drivers/video/fbdev/msm/mdss_mdp_util.c b/drivers/video/fbdev/msm/mdss_mdp_util.c
index a8997e3a2b43..957075185183 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_util.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_util.c
@@ -388,6 +388,31 @@ int mdss_mdp_data_check(struct mdss_mdp_data *data,
 	return 0;
 }
 
+void mdss_mdp_data_calc_offset(struct mdss_mdp_data *data, u16 x, u16 y,
+	struct mdss_mdp_plane_sizes *ps, struct mdss_mdp_format_params *fmt)
+{
+	if ((x == 0) && (y == 0))
+		return;
+
+	data->p[0].addr += y * ps->ystride[0];
+
+	if (data->num_planes == 1) {
+		data->p[0].addr += x * fmt->bpp;
+	} else {
+		u8 hmap[] = { 1, 2, 1, 2 };
+		u8 vmap[] = { 1, 1, 2, 2 };
+		u16 xoff = x / hmap[fmt->chroma_sample];
+		u16 yoff = y / vmap[fmt->chroma_sample];
+
+		data->p[0].addr += x;
+		data->p[1].addr += xoff + (yoff * ps->ystride[1]);
+		if (data->num_planes == 2) /* pseudo planar */
+			data->p[1].addr += xoff;
+		else /* planar */
+			data->p[2].addr += xoff + (yoff * ps->ystride[2]);
+	}
+}
+
 int mdss_mdp_put_img(struct mdss_mdp_img_data *data)
 {
 	struct ion_client *iclient = mdss_get_ionclient();