mdss: display: Add writeback support for WFD

- Implement APIs used to support wifi display (WFD) based on ioctls and
  functions declared in linux/msm_mdp.h.
- Restructure writeback interface file to support custom output formats
  and take buffers from WFD APIs.

Change-Id: I3e6b75bd8b7d73eb20c97899de1891bbd85e9fa4
Signed-off-by: Adrian Salido-Moreno <adrianm@codeaurora.org>
[cip@codeaurora.org: Moved mdss_mdp_wb.c file location]
Signed-off-by: Clarence Ip <cip@codeaurora.org>

9 files changed, 698 insertions, 108 deletions

diff --git a/drivers/video/fbdev/msm/Makefile b/drivers/video/fbdev/msm/Makefile
index 780e0c64009b..492437e62e8c 100644
--- a/drivers/video/fbdev/msm/Makefile
+++ b/drivers/video/fbdev/msm/Makefile
@@ -4,6 +4,7 @@ mdss-mdp-objs += mdss_mdp_intf_video.o
 mdss-mdp-objs += mdss_mdp_intf_writeback.o
 mdss-mdp-objs += mdss_mdp_rotator.o
 mdss-mdp-objs += mdss_mdp_overlay.o
+mdss-mdp-objs += mdss_mdp_wb.o
 obj-$(CONFIG_FB_MSM_MDSS) += mdss-mdp.o
 obj-$(CONFIG_FB_MSM_MDSS) += mdss_fb.o
 obj-$(CONFIG_FB_MSM_MDSS_WRITEBACK) += mdss_wb.o
diff --git a/drivers/video/fbdev/msm/mdss_fb.c b/drivers/video/fbdev/msm/mdss_fb.c
index 2a53328c6851..1901b9569a36 100644
--- a/drivers/video/fbdev/msm/mdss_fb.c
+++ b/drivers/video/fbdev/msm/mdss_fb.c
@@ -1158,6 +1158,20 @@ static int mdss_fb_ioctl(struct fb_info *info, unsigned int cmd,
 	return ret;
 }
 
+struct fb_info *msm_fb_get_writeback_fb(void)
+{
+	int c = 0;
+	for (c = 0; c < fbi_list_index; ++c) {
+		struct msm_fb_data_type *mfd;
+		mfd = (struct msm_fb_data_type *)fbi_list[c]->par;
+		if (mfd->panel.type == WRITEBACK_PANEL)
+			return fbi_list[c];
+	}
+
+	return NULL;
+}
+EXPORT_SYMBOL(msm_fb_get_writeback_fb);
+
 int mdss_register_panel(struct mdss_panel_data *pdata)
 {
 	struct platform_device *mdss_fb_dev = NULL;
diff --git a/drivers/video/fbdev/msm/mdss_fb.h b/drivers/video/fbdev/msm/mdss_fb.h
index 4d2fec3d4fcc..98c967d5f558 100644
--- a/drivers/video/fbdev/msm/mdss_fb.h
+++ b/drivers/video/fbdev/msm/mdss_fb.h
@@ -55,6 +55,7 @@ struct msm_fb_data_type {
 
 	int op_enable;
 	u32 fb_imgType;
+	u32 dst_format;
 
 	int hw_refresh;
 
@@ -90,6 +91,7 @@ struct msm_fb_data_type {
 	struct ion_client *iclient;
 
 	struct mdss_mdp_ctl *ctl;
+	struct mdss_mdp_wb *wb;
 };
 
 int mdss_fb_get_phys_info(unsigned long *start, unsigned long *len, int fb_num);
diff --git a/drivers/video/fbdev/msm/mdss_mdp.h b/drivers/video/fbdev/msm/mdss_mdp.h
index 28bc55442837..cc516ce0042a 100644
--- a/drivers/video/fbdev/msm/mdss_mdp.h
+++ b/drivers/video/fbdev/msm/mdss_mdp.h
@@ -247,6 +247,12 @@ struct mdss_mdp_pipe {
 	unsigned long smp[MAX_PLANES];
 };
 
+struct mdss_mdp_writeback_arg {
+	struct mdss_mdp_data *data;
+	void (*callback_fnc) (void *arg);
+	void *priv_data;
+};
+
 static inline void mdss_mdp_ctl_write(struct mdss_mdp_ctl *ctl,
 				      u32 reg, u32 val)
 {
@@ -313,4 +319,7 @@ int mdss_mdp_put_img(struct mdss_mdp_img_data *data);
 int mdss_mdp_get_img(struct ion_client *iclient, struct msmfb_data *img,
 		     struct mdss_mdp_img_data *data);
 
+int mdss_mdp_wb_kickoff(struct mdss_mdp_ctl *ctl);
+int mdss_mdp_wb_ioctl_handler(struct msm_fb_data_type *mfd, u32 cmd, void *arg);
+
 #endif /* MDSS_MDP_H */
diff --git a/drivers/video/fbdev/msm/mdss_mdp_intf_writeback.c b/drivers/video/fbdev/msm/mdss_mdp_intf_writeback.c
index d7d22958fa83..cd6bd14173db 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_intf_writeback.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_intf_writeback.c
@@ -39,9 +39,12 @@ struct mdss_mdp_writeback_ctx {
 	u16 height;
 	u8 rot90;
 
-	struct completion comp;
+	int initialized;
+
 	struct mdss_mdp_plane_sizes dst_planes;
-	struct mdss_mdp_data wb_data;
+
+	void (*callback_fnc) (void *arg);
+	void *callback_arg;
 };
 
 static struct mdss_mdp_writeback_ctx wb_ctx_list[MDSS_MDP_MAX_WRITEBACK] = {
@@ -72,8 +75,6 @@ static struct mdss_mdp_writeback_ctx wb_ctx_list[MDSS_MDP_MAX_WRITEBACK] = {
 	},
 };
 
-static void *videomemory;
-
 static int mdss_mdp_writeback_addr_setup(struct mdss_mdp_writeback_ctx *ctx,
 					 struct mdss_mdp_data *data)
 {
@@ -101,7 +102,8 @@ static int mdss_mdp_writeback_format_setup(struct mdss_mdp_writeback_ctx *ctx)
 {
 	struct mdss_mdp_format_params *fmt;
 	u32 dst_format, pattern, ystride0, ystride1, outsize, chroma_samp;
-	int off, ret;
+	int off;
+	u32 opmode = ctx->opmode;
 
 	pr_debug("wb_num=%d format=%d\n", ctx->wb_num, ctx->format);
 
@@ -111,11 +113,30 @@ static int mdss_mdp_writeback_format_setup(struct mdss_mdp_writeback_ctx *ctx)
 	fmt = mdss_mdp_get_format_params(ctx->format);
 	if (!fmt) {
 		pr_err("wb format=%d not supported\n", ctx->format);
-		return ret;
+		return -EINVAL;
 	}
 
 	chroma_samp = fmt->chroma_sample;
-	if (ctx->rot90) {
+
+	if (ctx->type != MDSS_MDP_WRITEBACK_TYPE_ROTATOR && fmt->is_yuv) {
+		mdss_mdp_csc_setup(MDSS_MDP_BLOCK_WB, ctx->wb_num, 0,
+				   MDSS_MDP_CSC_RGB2YUV);
+		opmode |= (1 << 8) |	/* CSC_EN */
+			  (0 << 9) |	/* SRC_DATA=RGB */
+			  (1 << 10);	/* DST_DATA=YCBCR */
+
+		switch (chroma_samp) {
+		case MDSS_MDP_CHROMA_RGB:
+		case MDSS_MDP_CHROMA_420:
+		case MDSS_MDP_CHROMA_H2V1:
+			opmode |= (chroma_samp << 11);
+			break;
+		case MDSS_MDP_CHROMA_H1V2:
+		default:
+			pr_err("unsupported wb chroma samp=%d\n", chroma_samp);
+			return -EINVAL;
+		}
+	} else if (ctx->rot90) {
 		if (chroma_samp == MDSS_MDP_CHROMA_H2V1)
 			chroma_samp = MDSS_MDP_CHROMA_H1V2;
 		else if (chroma_samp == MDSS_MDP_CHROMA_H1V2)
@@ -147,7 +168,7 @@ static int mdss_mdp_writeback_format_setup(struct mdss_mdp_writeback_ctx *ctx)
 
 	off = MDSS_MDP_REG_WB_OFFSET(ctx->wb_num);
 	MDSS_MDP_REG_WRITE(off + MDSS_MDP_REG_WB_DST_FORMAT, dst_format);
-	MDSS_MDP_REG_WRITE(off + MDSS_MDP_REG_WB_DST_OP_MODE, ctx->opmode);
+	MDSS_MDP_REG_WRITE(off + MDSS_MDP_REG_WB_DST_OP_MODE, opmode);
 	MDSS_MDP_REG_WRITE(off + MDSS_MDP_REG_WB_DST_PACK_PATTERN, pattern);
 	MDSS_MDP_REG_WRITE(off + MDSS_MDP_REG_WB_DST_YSTRIDE0, ystride0);
 	MDSS_MDP_REG_WRITE(off + MDSS_MDP_REG_WB_DST_YSTRIDE1, ystride1);
@@ -156,32 +177,24 @@ static int mdss_mdp_writeback_format_setup(struct mdss_mdp_writeback_ctx *ctx)
 	return 0;
 }
 
-static int mdss_mdp_writeback_wfd_setup(struct mdss_mdp_ctl *ctl,
-					struct mdss_mdp_writeback_ctx *ctx)
+static int mdss_mdp_writeback_prepare_wfd(struct mdss_mdp_ctl *ctl, void *arg)
 {
-	struct msm_fb_data_type *mfd;
-	struct fb_info *fbi;
+	struct mdss_mdp_writeback_ctx *ctx;
 	int ret;
-	u32 plane_size;
 
-	mfd = ctl->mfd;
-	fbi = mfd->fbi;
+	ctx = (struct mdss_mdp_writeback_ctx *) ctl->priv_data;
+	if (!ctx)
+		return -ENODEV;
+
+	if (ctx->initialized) /* already set */
+		return 0;
 
-	pr_debug("setup ctl=%d\n", ctl->num);
+	pr_debug("wfd setup ctl=%d\n", ctl->num);
 
 	ctx->opmode = 0;
 	ctx->format = ctl->dst_format;
-	ctx->width = fbi->var.xres;
-	ctx->height = fbi->var.yres;
-
-	plane_size = ctx->width * ctx->height * fbi->var.bits_per_pixel / 8;
-
-	videomemory = (void *) fbi->fix.smem_start + fbi->fix.smem_len -
-		      plane_size;
-
-	ctx->wb_data.num_planes = 1;
-	ctx->wb_data.p[0].addr = (u32) videomemory;
-	ctx->wb_data.p[0].len = plane_size;
+	ctx->width = ctl->width;
+	ctx->height = ctl->height;
 
 	ret = mdss_mdp_writeback_format_setup(ctx);
 	if (ret) {
@@ -189,16 +202,30 @@ static int mdss_mdp_writeback_wfd_setup(struct mdss_mdp_ctl *ctl,
 		return ret;
 	}
 
-	ctl->flush_bits |=  BIT(16); /* WB */
+	ctx->initialized = true;
 
 	return 0;
 }
 
-static int mdss_mdp_writeback_rot_setup(struct mdss_mdp_ctl *ctl,
-					struct mdss_mdp_writeback_ctx *ctx,
-					struct mdss_mdp_rotator_session *rot)
+static int mdss_mdp_writeback_prepare_rot(struct mdss_mdp_ctl *ctl, void *arg)
 {
-	pr_debug("rotator wb_num=%d\n", ctx->wb_num);
+	struct mdss_mdp_writeback_ctx *ctx;
+	struct mdss_mdp_writeback_arg *wb_args;
+	struct mdss_mdp_rotator_session *rot;
+
+	ctx = (struct mdss_mdp_writeback_ctx *) ctl->priv_data;
+	if (!ctx)
+		return -ENODEV;
+	wb_args = (struct mdss_mdp_writeback_arg *) arg;
+	if (!wb_args)
+		return -ENOENT;
+
+	rot = (struct mdss_mdp_rotator_session *) wb_args->priv_data;
+	if (!rot) {
+		pr_err("unable to retrieve rot session ctl=%d\n", ctl->num);
+		return -ENODEV;
+	}
+	pr_debug("rot setup wb_num=%d\n", ctx->wb_num);
 
 	ctx->opmode = BIT(6); /* ROT EN */
 	if (ROT_BLK_SIZE == 128)
@@ -238,27 +265,6 @@ static int mdss_mdp_writeback_stop(struct mdss_mdp_ctl *ctl)
 	return 0;
 }
 
-static int mdss_mdp_writeback_prepare(struct mdss_mdp_ctl *ctl, void *arg)
-{
-	struct mdss_mdp_writeback_ctx *ctx;
-	ctx = (struct mdss_mdp_writeback_ctx *) ctl->priv_data;
-	if (!ctx)
-		return -ENODEV;
-
-	if (ctx->type == MDSS_MDP_WRITEBACK_TYPE_ROTATOR) {
-		struct mdss_mdp_rotator_session *rot;
-		rot = (struct mdss_mdp_rotator_session *) arg;
-		if (!rot) {
-			pr_err("unable to retrieve rot session ctl=%d\n",
-			       ctl->num);
-			return -ENODEV;
-		}
-		mdss_mdp_writeback_rot_setup(ctl, ctx, rot);
-	}
-
-	return 0;
-}
-
 static void mdss_mdp_writeback_intr_done(void *arg)
 {
 	struct mdss_mdp_writeback_ctx *ctx;
@@ -274,14 +280,14 @@ static void mdss_mdp_writeback_intr_done(void *arg)
 	mdss_mdp_irq_disable_nosync(ctx->intr_type, ctx->intf_num);
 	mdss_mdp_clk_ctrl(MDP_BLOCK_POWER_OFF, true);
 
-	complete_all(&ctx->comp);
+	if (ctx->callback_fnc)
+		ctx->callback_fnc(ctx->callback_arg);
 }
 
 static int mdss_mdp_writeback_display(struct mdss_mdp_ctl *ctl, void *arg)
 {
 	struct mdss_mdp_writeback_ctx *ctx;
-	struct mdss_mdp_rotator_session *rot = NULL;
-	struct mdss_mdp_data *wb_data;
+	struct mdss_mdp_writeback_arg *wb_args;
 	u32 flush_bits;
 	int ret;
 
@@ -289,28 +295,22 @@ static int mdss_mdp_writeback_display(struct mdss_mdp_ctl *ctl, void *arg)
 	if (!ctx)
 		return -ENODEV;
 
-	if (ctx->type == MDSS_MDP_WRITEBACK_TYPE_ROTATOR) {
-		rot = (struct mdss_mdp_rotator_session *) arg;
-		if (!rot) {
-			pr_err("unable to retrieve rot session ctl=%d\n",
-			       ctl->num);
-			return -ENODEV;
-		}
-		wb_data = rot->dst_data;
-	} else {
-		wb_data = &ctx->wb_data;
-	}
+	wb_args = (struct mdss_mdp_writeback_arg *) arg;
+	if (!wb_args)
+		return -ENOENT;
 
-	ret = mdss_mdp_writeback_addr_setup(ctx, wb_data);
+	ret = mdss_mdp_writeback_addr_setup(ctx, wb_args->data);
 	if (ret) {
 		pr_err("writeback data setup error ctl=%d\n", ctl->num);
 		return ret;
 	}
 
+	ctx->callback_fnc = wb_args->callback_fnc;
+	ctx->callback_arg = wb_args->priv_data;
+
 	flush_bits = BIT(16); /* WB */
 	mdss_mdp_ctl_write(ctl, MDSS_MDP_REG_CTL_FLUSH, flush_bits);
 
-	INIT_COMPLETION(ctx->comp);
 	mdss_mdp_set_intr_callback(ctx->intr_type, ctx->intf_num,
 				   mdss_mdp_writeback_intr_done, ctx);
 	mdss_mdp_irq_enable(ctx->intr_type, ctx->intf_num);
@@ -319,17 +319,6 @@ static int mdss_mdp_writeback_display(struct mdss_mdp_ctl *ctl, void *arg)
 	mdss_mdp_ctl_write(ctl, MDSS_MDP_REG_CTL_START, 1);
 	wmb();
 
-	if (rot) {
-		pr_debug("rotator kickoff wb_num=%d\n", ctx->wb_num);
-		mutex_lock(&rot->lock);
-		rot->comp = &ctx->comp;
-		rot->busy = 1;
-		mutex_unlock(&rot->lock);
-	} else {
-		pr_debug("writeback kickoff wb_num=%d\n", ctx->wb_num);
-		wait_for_completion_interruptible(&ctx->comp);
-	}
-
 	return 0;
 }
 
@@ -355,16 +344,12 @@ int mdss_mdp_writeback_start(struct mdss_mdp_ctl *ctl)
 	}
 	ctl->priv_data = ctx;
 	ctx->wb_num = ctl->num;	/* wb num should match ctl num */
+	ctx->initialized = false;
 
-	init_completion(&ctx->comp);
-
-	if (ctx->type == MDSS_MDP_WRITEBACK_TYPE_WFD)
-		ret = mdss_mdp_writeback_wfd_setup(ctl, ctx);
-	else if (ctx->type == MDSS_MDP_WRITEBACK_TYPE_ROTATOR)
-		ctl->prepare_fnc = mdss_mdp_writeback_prepare;
-	else /* line mode not supported */
-		return -ENOSYS;
-
+	if (ctx->type == MDSS_MDP_WRITEBACK_TYPE_ROTATOR)
+		ctl->prepare_fnc = mdss_mdp_writeback_prepare_rot;
+	else /* wfd or line mode */
+		ctl->prepare_fnc = mdss_mdp_writeback_prepare_wfd;
 	ctl->stop_fnc = mdss_mdp_writeback_stop;
 	ctl->display_fnc = mdss_mdp_writeback_display;
 
diff --git a/drivers/video/fbdev/msm/mdss_mdp_overlay.c b/drivers/video/fbdev/msm/mdss_mdp_overlay.c
index a0b31cd49b6f..33e4543a06fb 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_overlay.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_overlay.c
@@ -793,6 +793,11 @@ static int mdss_mdp_overlay_ioctl_handler(struct msm_fb_data_type *mfd,
 			ret = 0;
 		}
 		break;
+
+	default:
+		if (mfd->panel_info.type == WRITEBACK_PANEL)
+			ret = mdss_mdp_wb_ioctl_handler(mfd, cmd, argp);
+		break;
 	}
 
 	return ret;
@@ -809,7 +814,11 @@ int mdss_mdp_overlay_init(struct msm_fb_data_type *mfd)
 	mfd->cursor_update = mdss_mdp_hw_cursor_update;
 	mfd->dma_fnc = mdss_mdp_overlay_pan_display;
 	mfd->ioctl_handler = mdss_mdp_overlay_ioctl_handler;
-	mfd->kickoff_fnc = mdss_mdp_overlay_kickoff;
+
+	if (mfd->panel_info.type == WRITEBACK_PANEL)
+		mfd->kickoff_fnc = mdss_mdp_wb_kickoff;
+	else
+		mfd->kickoff_fnc = mdss_mdp_overlay_kickoff;
 
 	return 0;
 }
diff --git a/drivers/video/fbdev/msm/mdss_mdp_rotator.c b/drivers/video/fbdev/msm/mdss_mdp_rotator.c
index 4d2e87faec0d..a23ec87255fe 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_rotator.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_rotator.c
@@ -39,6 +39,7 @@ struct mdss_mdp_rotator_session *mdss_mdp_rotator_session_alloc(void)
 			rot->ref_cnt++;
 			rot->session_id = i | MDSS_MDP_ROT_SESSION_MASK;
 			mutex_init(&rot->lock);
+			init_completion(&rot->comp);
 			break;
 		}
 	}
@@ -65,19 +66,6 @@ struct mdss_mdp_rotator_session *mdss_mdp_rotator_session_get(u32 session_id)
 	return NULL;
 }
 
-static int mdss_mdp_rotator_busy_wait(struct mdss_mdp_rotator_session *rot)
-{
-	mutex_lock(&rot->lock);
-	if (rot->busy) {
-		pr_debug("waiting for rot=%d to complete\n", rot->pipe->num);
-		wait_for_completion_interruptible(rot->comp);
-		rot->busy = 0;
-	}
-	mutex_unlock(&rot->lock);
-
-	return 0;
-}
-
 static struct mdss_mdp_pipe *mdss_mdp_rotator_pipe_alloc(void)
 {
 	struct mdss_mdp_mixer *mixer;
@@ -110,6 +98,52 @@ done:
 	return pipe;
 }
 
+static int mdss_mdp_rotator_busy_wait(struct mdss_mdp_rotator_session *rot)
+{
+	mutex_lock(&rot->lock);
+	if (rot->busy) {
+		pr_debug("waiting for rot=%d to complete\n", rot->pipe->num);
+		wait_for_completion_interruptible(&rot->comp);
+		rot->busy = false;
+
+	}
+	mutex_unlock(&rot->lock);
+
+	return 0;
+}
+
+static void mdss_mdp_rotator_callback(void *arg)
+{
+	struct mdss_mdp_rotator_session *rot;
+
+	rot = (struct mdss_mdp_rotator_session *) arg;
+	if (rot)
+		complete(&rot->comp);
+}
+
+static int mdss_mdp_rotator_kickoff(struct mdss_mdp_ctl *ctl,
+				    struct mdss_mdp_rotator_session *rot,
+				    struct mdss_mdp_data *dst_data)
+{
+	int ret;
+	struct mdss_mdp_writeback_arg wb_args = {
+		.callback_fnc = mdss_mdp_rotator_callback,
+		.data = dst_data,
+		.priv_data = rot,
+	};
+
+	mutex_lock(&rot->lock);
+	INIT_COMPLETION(rot->comp);
+	rot->busy = true;
+	ret = mdss_mdp_display_commit(ctl, &wb_args);
+	if (ret) {
+		rot->busy = false;
+		pr_err("problem with kickoff rot pipe=%d", rot->pipe->num);
+	}
+	mutex_unlock(&rot->lock);
+	return ret;
+}
+
 static int mdss_mdp_rotator_pipe_dequeue(struct mdss_mdp_rotator_session *rot)
 {
 	if (rot->pipe) {
@@ -182,15 +216,13 @@ int mdss_mdp_rotator_queue(struct mdss_mdp_rotator_session *rot,
 		rot_pipe->params_changed++;
 	}
 
-	rot->dst_data = dst_data;
-
 	ret = mdss_mdp_pipe_queue_data(rot->pipe, src_data);
 	if (ret) {
 		pr_err("unable to queue rot data\n");
 		goto done;
 	}
 
-	ret = mdss_mdp_display_commit(ctl, rot);
+	ret = mdss_mdp_rotator_kickoff(ctl, rot, dst_data);
 
 done:
 	mutex_unlock(&rotator_lock);
diff --git a/drivers/video/fbdev/msm/mdss_mdp_rotator.h b/drivers/video/fbdev/msm/mdss_mdp_rotator.h
index 5e5007b27233..7d39c72f8585 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_rotator.h
+++ b/drivers/video/fbdev/msm/mdss_mdp_rotator.h
@@ -33,12 +33,11 @@ struct mdss_mdp_rotator_session {
 
 	u32 bwc_mode;
 	struct mdss_mdp_pipe *pipe;
-	struct mdss_mdp_data *dst_data;
 
 	struct mutex lock;
+	struct completion comp;
 	u8 busy;
 	u8 no_wait;
-	struct completion *comp;
 
 	struct list_head head;
 };
diff --git a/drivers/video/fbdev/msm/mdss_mdp_wb.c b/drivers/video/fbdev/msm/mdss_mdp_wb.c
new file mode 100644
index 000000000000..82bcf57b3135
--- /dev/null
+++ b/drivers/video/fbdev/msm/mdss_mdp_wb.c
@@ -0,0 +1,539 @@
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#define pr_fmt(fmt)	"%s: " fmt, __func__
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/major.h>
+#include <linux/module.h>
+#include <linux/uaccess.h>
+
+#include "mdss_mdp.h"
+#include "mdss_fb.h"
+
+#define DEBUG_WRITEBACK
+
+enum mdss_mdp_wb_state {
+	WB_OPEN,
+	WB_START,
+	WB_STOPING,
+	WB_STOP
+};
+
+struct mdss_mdp_wb {
+	u32 fb_ndx;
+	struct mutex lock;
+	struct list_head busy_queue;
+	struct list_head free_queue;
+	struct list_head register_queue;
+	wait_queue_head_t wait_q;
+	u32 state;
+};
+
+enum mdss_mdp_wb_node_state {
+	REGISTERED,
+	IN_FREE_QUEUE,
+	IN_BUSY_QUEUE,
+	WITH_CLIENT
+};
+
+struct mdss_mdp_wb_data {
+	struct list_head registered_entry;
+	struct list_head active_entry;
+	struct msmfb_data buf_info;
+	struct mdss_mdp_data buf_data;
+	int state;
+};
+
+static DEFINE_MUTEX(mdss_mdp_wb_buf_lock);
+static struct mdss_mdp_wb mdss_mdp_wb_info;
+
+#ifdef DEBUG_WRITEBACK
+/* for debugging: writeback output buffer to framebuffer memory */
+static inline
+struct mdss_mdp_data *mdss_mdp_wb_debug_buffer(struct msm_fb_data_type *mfd)
+{
+	static void *videomemory;
+	static void *mdss_wb_mem;
+	static struct mdss_mdp_data buffer = {
+		.num_planes = 1,
+	};
+
+	struct fb_info *fbi;
+	int img_size;
+	int offset;
+
+
+	fbi = mfd->fbi;
+	img_size = fbi->var.xres * fbi->var.yres * fbi->var.bits_per_pixel / 8;
+	offset = fbi->fix.smem_len - img_size;
+
+	videomemory = fbi->screen_base + offset;
+	mdss_wb_mem = (void *)(fbi->fix.smem_start + offset);
+
+	buffer.p[0].addr = fbi->fix.smem_start + offset;
+	buffer.p[0].len = img_size;
+
+	return &buffer;
+}
+#else
+static inline
+struct mdss_mdp_data *mdss_mdp_wb_debug_buffer(struct msm_fb_data_type *mfd)
+{
+	return NULL;
+}
+#endif
+
+static int mdss_mdp_wb_init(struct msm_fb_data_type *mfd)
+{
+	struct mdss_mdp_wb *wb;
+
+	mutex_lock(&mdss_mdp_wb_buf_lock);
+	wb = mfd->wb;
+	if (wb == NULL) {
+		wb = &mdss_mdp_wb_info;
+		wb->fb_ndx = mfd->index;
+		mfd->wb = wb;
+	} else if (mfd->index != wb->fb_ndx) {
+		pr_err("only one writeback intf supported at a time\n");
+		return -EMLINK;
+	} else {
+		pr_debug("writeback already initialized\n");
+	}
+
+	pr_debug("init writeback on fb%d\n", wb->fb_ndx);
+
+	mutex_init(&wb->lock);
+	INIT_LIST_HEAD(&wb->free_queue);
+	INIT_LIST_HEAD(&wb->busy_queue);
+	INIT_LIST_HEAD(&wb->register_queue);
+	wb->state = WB_OPEN;
+	init_waitqueue_head(&wb->wait_q);
+
+	mfd->wb = wb;
+	mutex_unlock(&mdss_mdp_wb_buf_lock);
+	return 0;
+}
+
+static int mdss_mdp_wb_terminate(struct msm_fb_data_type *mfd)
+{
+	struct mdss_mdp_wb *wb = mfd->wb;
+
+	if (!wb) {
+		pr_err("unable to terminate, writeback is not initialized\n");
+		return -ENODEV;
+	}
+
+	pr_debug("terminate writeback\n");
+
+	mutex_lock(&mdss_mdp_wb_buf_lock);
+	mutex_lock(&wb->lock);
+	if (!list_empty(&wb->register_queue)) {
+		struct mdss_mdp_wb_data *node, *temp;
+		list_for_each_entry_safe(node, temp, &wb->register_queue,
+					 registered_entry) {
+			list_del(&node->registered_entry);
+			kfree(node);
+		}
+	}
+	mutex_unlock(&wb->lock);
+
+	mfd->wb = NULL;
+	mutex_unlock(&mdss_mdp_wb_buf_lock);
+
+	return 0;
+}
+
+static int mdss_mdp_wb_start(struct msm_fb_data_type *mfd)
+{
+	struct mdss_mdp_wb *wb = mfd->wb;
+
+	if (!wb) {
+		pr_err("unable to start, writeback is not initialized\n");
+		return -ENODEV;
+	}
+
+	mutex_lock(&wb->lock);
+	wb->state = WB_START;
+	mutex_unlock(&wb->lock);
+	wake_up(&wb->wait_q);
+
+	return 0;
+}
+
+static int mdss_mdp_wb_stop(struct msm_fb_data_type *mfd)
+{
+	struct mdss_mdp_wb *wb = mfd->wb;
+
+	if (!wb) {
+		pr_err("unable to stop, writeback is not initialized\n");
+		return -ENODEV;
+	}
+
+	mutex_lock(&wb->lock);
+	wb->state = WB_STOPING;
+	mutex_unlock(&wb->lock);
+	wake_up(&wb->wait_q);
+
+	return 0;
+}
+
+static int mdss_mdp_wb_register_node(struct mdss_mdp_wb *wb,
+				     struct mdss_mdp_wb_data *node)
+{
+	node->state = REGISTERED;
+	list_add_tail(&node->registered_entry, &wb->register_queue);
+	if (!node) {
+		pr_err("Invalid wb node\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static struct mdss_mdp_wb_data *get_local_node(struct mdss_mdp_wb *wb,
+					       struct msmfb_data *data) {
+	struct mdss_mdp_wb_data *node;
+	struct mdss_mdp_img_data *buf;
+	int ret;
+
+	if (!data->iova)
+		return NULL;
+
+	if (!list_empty(&wb->register_queue)) {
+		list_for_each_entry(node, &wb->register_queue, registered_entry)
+		if (node->buf_info.iova == data->iova) {
+			pr_debug("found node iova=%x addr=%x\n",
+				 data->iova, node->buf_data.p[0].addr);
+			return node;
+		}
+	}
+
+	node = kzalloc(sizeof(struct mdss_mdp_wb_data), GFP_KERNEL);
+	if (node == NULL) {
+		pr_err("out of memory\n");
+		return NULL;
+	}
+
+	node->buf_data.num_planes = 1;
+	buf = &node->buf_data.p[0];
+	buf->addr = (u32) (data->iova + data->offset);
+	buf->len = UINT_MAX; /* trusted source */
+	ret = mdss_mdp_wb_register_node(wb, node);
+	if (IS_ERR_VALUE(ret)) {
+		pr_err("error registering wb node\n");
+		kfree(node);
+		return NULL;
+	}
+
+	pr_debug("register node iova=0x%x addr=0x%x\n", data->iova, buf->addr);
+
+	return node;
+}
+
+static struct mdss_mdp_wb_data *get_user_node(struct msm_fb_data_type *mfd,
+					      struct msmfb_data *data) {
+	struct mdss_mdp_wb *wb = mfd->wb;
+	struct mdss_mdp_wb_data *node;
+	struct mdss_mdp_img_data *buf;
+	int ret;
+
+	node = kzalloc(sizeof(struct mdss_mdp_wb_data), GFP_KERNEL);
+	if (node == NULL) {
+		pr_err("out of memory\n");
+		return NULL;
+	}
+
+	node->buf_data.num_planes = 1;
+	buf = &node->buf_data.p[0];
+	ret = mdss_mdp_get_img(mfd->iclient, data, buf);
+	if (IS_ERR_VALUE(ret)) {
+		pr_err("error getting buffer info\n");
+		goto register_fail;
+	}
+	memcpy(&node->buf_info, data, sizeof(*data));
+
+	ret = mdss_mdp_wb_register_node(wb, node);
+	if (IS_ERR_VALUE(ret)) {
+		pr_err("error registering wb node\n");
+		goto register_fail;
+	}
+
+	pr_debug("register node mem_id=%d offset=%u addr=0x%x len=%d\n",
+		 data->memory_id, data->offset, buf->addr, buf->len);
+
+	return node;
+
+register_fail:
+	kfree(node);
+	return NULL;
+}
+
+static int mdss_mdp_wb_queue(struct msm_fb_data_type *mfd,
+			     struct msmfb_data *data, int local)
+{
+	struct mdss_mdp_wb *wb = mfd->wb;
+	struct mdss_mdp_wb_data *node = NULL;
+	int ret = 0;
+
+	if (!wb) {
+		pr_err("unable to queue, writeback is not initialized\n");
+		return -ENODEV;
+	}
+
+	pr_debug("fb%d queue\n", wb->fb_ndx);
+
+	mutex_lock(&wb->lock);
+	if (local)
+		node = get_local_node(wb, data);
+	if (node == NULL)
+		node = get_user_node(mfd, data);
+
+	if (!node || node->state == IN_BUSY_QUEUE ||
+	    node->state == IN_FREE_QUEUE) {
+		pr_err("memory not registered or Buffer already with us\n");
+		ret = -EINVAL;
+	} else {
+		list_add_tail(&node->active_entry, &wb->free_queue);
+		node->state = IN_FREE_QUEUE;
+	}
+	mutex_unlock(&wb->lock);
+
+	return ret;
+}
+
+static int is_buffer_ready(struct mdss_mdp_wb *wb)
+{
+	int rc;
+	mutex_lock(&wb->lock);
+	rc = !list_empty(&wb->busy_queue) || (wb->state == WB_STOPING);
+	mutex_unlock(&wb->lock);
+
+	return rc;
+}
+
+static int mdss_mdp_wb_dequeue(struct msm_fb_data_type *mfd,
+			       struct msmfb_data *data)
+{
+	struct mdss_mdp_wb *wb = mfd->wb;
+	struct mdss_mdp_wb_data *node = NULL;
+	int ret;
+
+	if (!wb) {
+		pr_err("unable to dequeue, writeback is not initialized\n");
+		return -ENODEV;
+	}
+
+	ret = wait_event_interruptible(wb->wait_q, is_buffer_ready(wb));
+	if (ret) {
+		pr_err("failed to get dequeued buffer\n");
+		return -ENOBUFS;
+	}
+
+	mutex_lock(&wb->lock);
+	if (wb->state == WB_STOPING) {
+		pr_debug("wfd stopped\n");
+		wb->state = WB_STOP;
+		ret = -ENOBUFS;
+	} else if (!list_empty(&wb->busy_queue)) {
+		struct mdss_mdp_img_data *buf;
+		node = list_first_entry(&wb->busy_queue,
+					struct mdss_mdp_wb_data,
+					active_entry);
+		list_del(&node->active_entry);
+		node->state = WITH_CLIENT;
+		memcpy(data, &node->buf_info, sizeof(*data));
+
+		buf = &node->buf_data.p[0];
+		pr_debug("found node addr=%x len=%d\n", buf->addr, buf->len);
+	} else {
+		pr_debug("node is NULL, wait for next\n");
+		ret = -ENOBUFS;
+	}
+	mutex_unlock(&wb->lock);
+	return 0;
+}
+
+static void mdss_mdp_wb_callback(void *arg)
+{
+	if (arg)
+		complete((struct completion *) arg);
+}
+
+int mdss_mdp_wb_kickoff(struct mdss_mdp_ctl *ctl)
+{
+	struct mdss_mdp_wb *wb;
+	struct mdss_mdp_wb_data *node = NULL;
+	int ret = 0;
+	DECLARE_COMPLETION_ONSTACK(comp);
+	struct mdss_mdp_writeback_arg wb_args = {
+		.callback_fnc = mdss_mdp_wb_callback,
+		.priv_data = &comp,
+	};
+
+	if (!ctl || !ctl->mfd)
+		return -ENODEV;
+
+	mutex_lock(&mdss_mdp_wb_buf_lock);
+	wb = ctl->mfd->wb;
+	if (wb) {
+		mutex_lock(&wb->lock);
+		if (!list_empty(&wb->free_queue) && wb->state != WB_STOPING &&
+		    wb->state != WB_STOP) {
+			node = list_first_entry(&wb->free_queue,
+						struct mdss_mdp_wb_data,
+						active_entry);
+			list_del(&node->active_entry);
+			node->state = IN_BUSY_QUEUE;
+			wb_args.data = &node->buf_data;
+		} else {
+			pr_debug("unable to get buf wb state=%d\n", wb->state);
+		}
+		mutex_unlock(&wb->lock);
+	}
+
+	if (wb_args.data == NULL)
+		wb_args.data = mdss_mdp_wb_debug_buffer(ctl->mfd);
+
+	if (wb_args.data == NULL) {
+		pr_err("unable to get writeback buf ctl=%d\n", ctl->num);
+		ret = -ENOMEM;
+		goto kickoff_fail;
+	}
+
+	ret = mdss_mdp_display_commit(ctl, &wb_args);
+	if (ret) {
+		pr_err("error on commit ctl=%d\n", ctl->num);
+		goto kickoff_fail;
+	}
+
+	wait_for_completion_interruptible(&comp);
+	if (wb && node) {
+		mutex_lock(&wb->lock);
+		list_add_tail(&node->active_entry, &wb->busy_queue);
+		mutex_unlock(&wb->lock);
+		wake_up(&wb->wait_q);
+	}
+
+kickoff_fail:
+	mutex_unlock(&mdss_mdp_wb_buf_lock);
+	return ret;
+}
+
+int mdss_mdp_wb_ioctl_handler(struct msm_fb_data_type *mfd, u32 cmd, void *arg)
+{
+	struct msmfb_data data;
+	int ret = -ENOSYS;
+
+	switch (cmd) {
+	case MSMFB_WRITEBACK_INIT:
+		ret = mdss_mdp_wb_init(mfd);
+		break;
+	case MSMFB_WRITEBACK_START:
+		ret = mdss_mdp_wb_start(mfd);
+		break;
+	case MSMFB_WRITEBACK_STOP:
+		ret = mdss_mdp_wb_stop(mfd);
+		break;
+	case MSMFB_WRITEBACK_QUEUE_BUFFER:
+		if (!copy_from_user(&data, arg, sizeof(data))) {
+			ret = mdss_mdp_wb_queue(mfd, arg, false);
+		} else {
+			pr_err("wb queue buf failed on copy_from_user\n");
+			ret = -EFAULT;
+		}
+		break;
+	case MSMFB_WRITEBACK_DEQUEUE_BUFFER:
+		if (!copy_from_user(&data, arg, sizeof(data))) {
+			ret = mdss_mdp_wb_dequeue(mfd, arg);
+		} else {
+			pr_err("wb dequeue buf failed on copy_from_user\n");
+			ret = -EFAULT;
+		}
+		break;
+	case MSMFB_WRITEBACK_TERMINATE:
+		ret = mdss_mdp_wb_terminate(mfd);
+		break;
+	}
+
+	return ret;
+}
+
+int msm_fb_writeback_start(struct fb_info *info)
+{
+	struct msm_fb_data_type *mfd = (struct msm_fb_data_type *) info->par;
+
+	if (!mfd)
+		return -ENODEV;
+
+	return mdss_mdp_wb_start(mfd);
+}
+EXPORT_SYMBOL(msm_fb_writeback_start);
+
+int msm_fb_writeback_queue_buffer(struct fb_info *info,
+				  struct msmfb_data *data)
+{
+	struct msm_fb_data_type *mfd = (struct msm_fb_data_type *) info->par;
+
+	if (!mfd)
+		return -ENODEV;
+
+	return mdss_mdp_wb_queue(mfd, data, true);
+}
+EXPORT_SYMBOL(msm_fb_writeback_queue_buffer);
+
+int msm_fb_writeback_dequeue_buffer(struct fb_info *info,
+				    struct msmfb_data *data)
+{
+	struct msm_fb_data_type *mfd = (struct msm_fb_data_type *) info->par;
+
+	if (!mfd)
+		return -ENODEV;
+
+	return mdss_mdp_wb_dequeue(mfd, data);
+}
+EXPORT_SYMBOL(msm_fb_writeback_dequeue_buffer);
+
+int msm_fb_writeback_stop(struct fb_info *info)
+{
+	struct msm_fb_data_type *mfd = (struct msm_fb_data_type *) info->par;
+
+	if (!mfd)
+		return -ENODEV;
+
+	return mdss_mdp_wb_stop(mfd);
+}
+EXPORT_SYMBOL(msm_fb_writeback_stop);
+
+int msm_fb_writeback_init(struct fb_info *info)
+{
+	struct msm_fb_data_type *mfd = (struct msm_fb_data_type *) info->par;
+
+	if (!mfd)
+		return -ENODEV;
+
+	return mdss_mdp_wb_init(mfd);
+}
+EXPORT_SYMBOL(msm_fb_writeback_init);
+
+int msm_fb_writeback_terminate(struct fb_info *info)
+{
+	struct msm_fb_data_type *mfd = (struct msm_fb_data_type *) info->par;
+
+	if (!mfd)
+		return -ENODEV;
+
+	return mdss_mdp_wb_terminate(mfd);
+}
+EXPORT_SYMBOL(msm_fb_writeback_terminate);