4 files changed, 372 insertions, 2 deletions

diff --git a/drivers/video/fbdev/msm/mdp3.c b/drivers/video/fbdev/msm/mdp3.c
index 1afb8448f52c..cdee6a866cf1 100644
--- a/drivers/video/fbdev/msm/mdp3.c
+++ b/drivers/video/fbdev/msm/mdp3.c
@@ -32,6 +32,9 @@
 #include <linux/spinlock.h>
 #include <linux/semaphore.h>
 #include <linux/uaccess.h>
+#include <linux/file.h>
+#include <linux/msm_kgsl.h>
+#include <linux/major.h>
 
 #include <mach/board.h>
 #include <mach/clk.h>
@@ -696,6 +699,108 @@ static int mdp3_parse_dt(struct platform_device *pdev)
 	return 0;
 }
 
+int mdp3_put_img(struct mdp3_img_data *data)
+{
+	struct ion_client *iclient = mdp3_res->ion_client;
+	int dom = (mdp3_res->domains + MDP3_IOMMU_DOMAIN)->domain_idx;
+
+	if (!data->srcp_file) {
+		pr_debug("No img to put\n");
+		return 0;
+	}
+	if (data->flags & MDP_BLIT_SRC_GEM) {
+		pr_debug("memory source MDP_BLIT_SRC_GEM\n");
+	} else if (data->flags & MDP_MEMORY_ID_TYPE_FB) {
+		pr_debug("fb mem buf=0x%x\n", data->addr);
+		fput_light(data->srcp_file, data->p_need);
+		data->srcp_file = NULL;
+	} else {
+		ion_unmap_iommu(iclient, data->srcp_ihdl, dom, 0);
+		ion_free(iclient, data->srcp_ihdl);
+		data->srcp_ihdl = NULL;
+	}
+	return 0;
+}
+
+int mdp3_get_img(struct msmfb_data *img, struct mdp3_img_data *data)
+{
+	struct file *file;
+	int ret = -EINVAL;
+	int fb_num;
+	unsigned long *start, *len;
+	struct ion_client *iclient = mdp3_res->ion_client;
+	int dom = (mdp3_res->domains + MDP3_IOMMU_DOMAIN)->domain_idx;
+
+	start = (unsigned long *) &data->addr;
+	len = (unsigned long *) &data->len;
+	data->flags |= img->flags;
+	data->p_need = 0;
+
+	if (img->flags & MDP_BLIT_SRC_GEM) {
+		data->srcp_file = NULL;
+		ret = kgsl_gem_obj_addr(img->memory_id, (int) img->priv,
+					&data->addr, &data->len);
+		if (!ret)
+			goto done;
+	}
+	if (img->flags & MDP_MEMORY_ID_TYPE_FB) {
+		file = fget_light(img->memory_id, &data->p_need);
+		if (file == NULL) {
+			pr_err("invalid framebuffer file (%d)\n",
+					img->memory_id);
+			return -EINVAL;
+		}
+		if (MAJOR(file->f_dentry->d_inode->i_rdev) == FB_MAJOR) {
+			fb_num = MINOR(file->f_dentry->d_inode->i_rdev);
+			ret = mdss_fb_get_phys_info(start, len, fb_num);
+			if (ret) {
+				pr_err("mdss_fb_get_phys_info() failed\n");
+				fput_light(file, data->p_need);
+				file = NULL;
+			}
+		} else {
+			pr_err("invalid FB_MAJOR\n");
+			fput_light(file, data->p_need);
+			file = NULL;
+			ret = -EINVAL;
+		}
+		data->srcp_file = file;
+		if (!ret)
+			goto done;
+	}
+	if (iclient) {
+		data->srcp_ihdl = ion_import_dma_buf(iclient, img->memory_id);
+		if (IS_ERR_OR_NULL(data->srcp_ihdl)) {
+			pr_err("error on ion_import_fd\n");
+			ret = PTR_ERR(data->srcp_ihdl);
+			data->srcp_ihdl = NULL;
+			return ret;
+		}
+
+		ret = ion_map_iommu(iclient, data->srcp_ihdl, dom,
+		    0, SZ_4K, 0, start, len, 0, 0);
+
+		if (IS_ERR_VALUE(ret)) {
+			ion_free(iclient, data->srcp_ihdl);
+			pr_err("failed to map ion handle (%d)\n", ret);
+			return ret;
+		}
+	}
+done:
+	if (!ret && (img->offset < data->len)) {
+		data->addr += img->offset;
+		data->len -= img->offset;
+
+		pr_debug("mem=%d ihdl=%p buf=0x%x len=0x%x\n", img->memory_id,
+			 data->srcp_ihdl, data->addr, data->len);
+	} else {
+		mdp3_put_img(data);
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
 static int mdp3_init(struct msm_fb_data_type *mfd)
 {
 	return mdp3_ctrl_init(mfd);
diff --git a/drivers/video/fbdev/msm/mdp3.h b/drivers/video/fbdev/msm/mdp3.h
index a90b23380d7d..54ab8b0a5db1 100644
--- a/drivers/video/fbdev/msm/mdp3.h
+++ b/drivers/video/fbdev/msm/mdp3.h
@@ -22,6 +22,7 @@
 #include <mach/iommu_domains.h>
 
 #include "mdp3_dma.h"
+#include "mdss_fb.h"
 
 enum  {
 	MDP3_CLK_AHB,
@@ -104,6 +105,15 @@ struct mdp3_hw_resource {
 	struct mdp3_intr_cb callbacks[MDP3_MAX_INTR];
 };
 
+struct mdp3_img_data {
+	u32 addr;
+	u32 len;
+	u32 flags;
+	int p_need;
+	struct file *srcp_file;
+	struct ion_handle *srcp_ihdl;
+};
+
 extern struct mdp3_hw_resource *mdp3_res;
 
 struct mdp3_dma *mdp3_get_dma_pipe(int capability);
@@ -115,6 +125,8 @@ int mdp3_set_intr_callback(u32 type, struct mdp3_intr_cb *cb);
 int mdp3_clk_set_rate(int clk_type, unsigned long clk_rate);
 int mdp3_clk_enable(int enable);
 int mdp3_bus_scale_set_quota(int client, u64 ab_quota, u64 ib_quota);
+int mdp3_put_img(struct mdp3_img_data *data);
+int mdp3_get_img(struct msmfb_data *img, struct mdp3_img_data *data);
 
 #define MDP3_REG_WRITE(addr, val) writel_relaxed(val, mdp3_res->mdp_base + addr)
 #define MDP3_REG_READ(addr) readl_relaxed(mdp3_res->mdp_base + addr)
diff --git a/drivers/video/fbdev/msm/mdp3_ctrl.c b/drivers/video/fbdev/msm/mdp3_ctrl.c
index fbaac8fcefc3..f5ac5e99abf2 100644
--- a/drivers/video/fbdev/msm/mdp3_ctrl.c
+++ b/drivers/video/fbdev/msm/mdp3_ctrl.c
@@ -26,6 +26,63 @@
 #define MDP_VSYNC_CLK_RATE	19200000
 #define VSYNC_PERIOD 16
 
+static void mdp3_ctrl_pan_display(struct msm_fb_data_type *mfd);
+
+static void mdp3_bufq_init(struct mdp3_buffer_queue *bufq)
+{
+	bufq->count = 0;
+	bufq->push_idx = 0;
+	bufq->pop_idx = 0;
+}
+
+static void mdp3_bufq_deinit(struct mdp3_buffer_queue *bufq)
+{
+	int count = bufq->count;
+
+	if (!count)
+		return;
+
+	while (count--) {
+		struct mdp3_img_data *data = &bufq->img_data[bufq->pop_idx];
+		bufq->pop_idx = (bufq->pop_idx + 1) % MDP3_MAX_BUF_QUEUE;
+		mdp3_put_img(data);
+	}
+	bufq->count = 0;
+	bufq->push_idx = 0;
+	bufq->pop_idx = 0;
+}
+
+static int mdp3_bufq_push(struct mdp3_buffer_queue *bufq,
+			struct mdp3_img_data *data)
+{
+	if (bufq->count >= MDP3_MAX_BUF_QUEUE) {
+		pr_err("bufq full\n");
+		return -EPERM;
+	}
+
+	bufq->img_data[bufq->push_idx] = *data;
+	bufq->push_idx = (bufq->push_idx + 1) % MDP3_MAX_BUF_QUEUE;
+	bufq->count++;
+	return 0;
+}
+
+static struct mdp3_img_data *mdp3_bufq_pop(struct mdp3_buffer_queue *bufq)
+{
+	struct mdp3_img_data *data;
+	if (bufq->count == 0)
+		return NULL;
+
+	data = &bufq->img_data[bufq->pop_idx];
+	bufq->count--;
+	bufq->pop_idx = (bufq->pop_idx + 1) % MDP3_MAX_BUF_QUEUE;
+	return data;
+}
+
+static int mdp3_bufq_count(struct mdp3_buffer_queue *bufq)
+{
+	return bufq->count;
+}
+
 void vsync_notify_handler(void *arg)
 {
 	struct mdp3_session_data *session = (struct mdp3_session_data *)arg;
@@ -409,6 +466,148 @@ off_error:
 	return 0;
 }
 
+static int mdp3_overlay_get(struct msm_fb_data_type *mfd,
+				struct mdp_overlay *req)
+{
+	int rc = 0;
+	struct mdp3_session_data *mdp3_session = mfd->mdp.private1;
+
+	mutex_lock(&mdp3_session->lock);
+
+	if (mdp3_session->overlay.id == req->id)
+		*req = mdp3_session->overlay;
+	else
+		rc = -EINVAL;
+
+	mutex_unlock(&mdp3_session->lock);
+
+	return rc;
+}
+
+static int mdp3_overlay_set(struct msm_fb_data_type *mfd,
+				struct mdp_overlay *req)
+{
+	int rc = 0;
+	struct mdp3_session_data *mdp3_session = mfd->mdp.private1;
+
+	mutex_lock(&mdp3_session->lock);
+
+	if (mdp3_session->overlay.id == req->id) {
+		mdp3_session->overlay = *req;
+		if (req->id == MSMFB_NEW_REQUEST) {
+			mdp3_session->overlay.id = 1;
+			req->id = 1;
+		}
+	} else {
+		rc = -EINVAL;
+	}
+	mutex_unlock(&mdp3_session->lock);
+
+	return rc;
+}
+
+static int mdp3_overlay_unset(struct msm_fb_data_type *mfd, int ndx)
+{
+	int rc = 0;
+	struct mdp3_session_data *mdp3_session = mfd->mdp.private1;
+
+	mdp3_ctrl_pan_display(mfd);
+
+	mutex_lock(&mdp3_session->lock);
+
+	if (mdp3_session->overlay.id == ndx && ndx == 1) {
+		mdp3_session->overlay.id = MSMFB_NEW_REQUEST;
+		mdp3_bufq_deinit(&mdp3_session->bufq_in);
+		mdp3_bufq_deinit(&mdp3_session->bufq_out);
+	} else {
+		rc = -EINVAL;
+	}
+
+	mutex_unlock(&mdp3_session->lock);
+
+	return rc;
+}
+
+static int mdp3_overlay_queue_buffer(struct msm_fb_data_type *mfd,
+					struct msmfb_overlay_data *req)
+{
+	int rc;
+	struct mdp3_session_data *mdp3_session = mfd->mdp.private1;
+	struct msmfb_data *img = &req->data;
+	struct mdp3_img_data data;
+
+	rc = mdp3_get_img(img, &data);
+	if (rc) {
+		pr_err("fail to get overlay buffer\n");
+		return rc;
+	}
+
+	rc = mdp3_bufq_push(&mdp3_session->bufq_in, &data);
+	if (rc) {
+		pr_err("fail to queue the overlay buffer, buffer drop\n");
+		mdp3_put_img(&data);
+		return rc;
+	}
+	return 0;
+}
+
+static int mdp3_overlay_play(struct msm_fb_data_type *mfd,
+				 struct msmfb_overlay_data *req)
+{
+	struct mdp3_session_data *mdp3_session = mfd->mdp.private1;
+	int rc = 0;
+
+	pr_debug("mdp3_overlay_play req id=%x mem_id=%d\n",
+		req->id, req->data.memory_id);
+
+	mutex_lock(&mdp3_session->lock);
+
+	if (mfd->panel_power_on)
+		rc = mdp3_overlay_queue_buffer(mfd, req);
+	else
+		rc = -EPERM;
+
+	mutex_unlock(&mdp3_session->lock);
+
+	return rc;
+}
+
+static int mdp3_ctrl_display_commit_kickoff(struct msm_fb_data_type *mfd)
+{
+	struct mdp3_session_data *mdp3_session;
+	struct mdp3_img_data *data;
+	int rc = 0;
+
+	if (!mfd || !mfd->mdp.private1)
+		return -EINVAL;
+
+	mdp3_session = mfd->mdp.private1;
+	if (!mdp3_session || !mdp3_session->dma)
+		return -EINVAL;
+
+	if (!mdp3_session->status) {
+		pr_err("%s, display off!\n", __func__);
+		return -EPERM;
+	}
+
+	mutex_lock(&mdp3_session->lock);
+
+	data = mdp3_bufq_pop(&mdp3_session->bufq_in);
+	if (data) {
+		mdp3_session->dma->update(mdp3_session->dma,
+			(void *)data->addr);
+		mdp3_bufq_push(&mdp3_session->bufq_out, data);
+	}
+
+	if (mdp3_bufq_count(&mdp3_session->bufq_out) > 1) {
+		data = mdp3_bufq_pop(&mdp3_session->bufq_out);
+		mdp3_put_img(data);
+	}
+
+	mutex_unlock(&mdp3_session->lock);
+	return rc;
+}
+
 static void mdp3_ctrl_pan_display(struct msm_fb_data_type *mfd)
 {
 	struct fb_info *fbi;
@@ -459,6 +658,9 @@ static int mdp3_get_metadata(struct msm_fb_data_type *mfd,
 		break;
 	case metadata_op_get_caps:
 		metadata->data.caps.mdp_rev = 304;
+		metadata->data.caps.rgb_pipes = 0;
+		metadata->data.caps.vig_pipes = 0;
+		metadata->data.caps.dma_pipes = 1;
 		break;
 	default:
 		pr_warn("Unsupported request to MDP META IOCTL.\n");
@@ -474,6 +676,8 @@ static int mdp3_ctrl_ioctl_handler(struct msm_fb_data_type *mfd,
 	int rc = -EINVAL;
 	struct mdp3_session_data *mdp3_session;
 	struct msmfb_metadata metadata;
+	struct mdp_overlay req;
+	struct msmfb_overlay_data ov_data;
 	int val;
 
 	pr_debug("mdp3_ctrl_ioctl_handler\n");
@@ -505,10 +709,42 @@ static int mdp3_ctrl_ioctl_handler(struct msm_fb_data_type *mfd,
 		if (!rc)
 			rc = copy_to_user(argp, &metadata, sizeof(metadata));
 		break;
+	case MSMFB_OVERLAY_GET:
+		rc = copy_from_user(&req, argp, sizeof(req));
+		if (!rc) {
+			rc = mdp3_overlay_get(mfd, &req);
+
+		if (!IS_ERR_VALUE(rc))
+			rc = copy_to_user(argp, &req, sizeof(req));
+		}
+		if (rc)
+			pr_err("OVERLAY_GET failed (%d)\n", rc);
+		break;
+	case MSMFB_OVERLAY_SET:
+		rc = copy_from_user(&req, argp, sizeof(req));
+		if (!rc) {
+			rc = mdp3_overlay_set(mfd, &req);
+
+		if (!IS_ERR_VALUE(rc))
+			rc = copy_to_user(argp, &req, sizeof(req));
+		}
+		if (rc)
+			pr_err("OVERLAY_SET failed (%d)\n", rc);
+		break;
+	case MSMFB_OVERLAY_UNSET:
+		if (!IS_ERR_VALUE(copy_from_user(&val, argp, sizeof(val))))
+			rc = mdp3_overlay_unset(mfd, val);
+		break;
+	case MSMFB_OVERLAY_PLAY:
+		rc = copy_from_user(&ov_data, argp, sizeof(ov_data));
+		if (!rc)
+			rc = mdp3_overlay_play(mfd, &ov_data);
+		if (rc)
+			pr_err("OVERLAY_PLAY failed (%d)\n", rc);
+		break;
 	default:
 		break;
 	}
-
 	return rc;
 }
 
@@ -527,7 +763,7 @@ int mdp3_ctrl_init(struct msm_fb_data_type *mfd)
 	mdp3_interface->cursor_update = NULL;
 	mdp3_interface->dma_fnc = mdp3_ctrl_pan_display;
 	mdp3_interface->ioctl_handler = mdp3_ctrl_ioctl_handler;
-	mdp3_interface->kickoff_fnc = NULL;
+	mdp3_interface->kickoff_fnc = mdp3_ctrl_display_commit_kickoff;
 
 	mdp3_session = kmalloc(sizeof(struct mdp3_session_data), GFP_KERNEL);
 	if (!mdp3_session) {
@@ -553,6 +789,10 @@ int mdp3_ctrl_init(struct msm_fb_data_type *mfd)
 
 	mdp3_session->panel = dev_get_platdata(&mfd->pdev->dev);
 	mdp3_session->status = 0;
+	mdp3_session->overlay.id = MSMFB_NEW_REQUEST;
+	mdp3_bufq_init(&mdp3_session->bufq_in);
+	mdp3_bufq_init(&mdp3_session->bufq_out);
+
 	mfd->mdp.private1 = mdp3_session;
 
 	rc = sysfs_create_group(&dev->kobj, &vsync_fs_attr_group);
diff --git a/drivers/video/fbdev/msm/mdp3_ctrl.h b/drivers/video/fbdev/msm/mdp3_ctrl.h
index af5139b50d89..fb3bd36133c9 100644
--- a/drivers/video/fbdev/msm/mdp3_ctrl.h
+++ b/drivers/video/fbdev/msm/mdp3_ctrl.h
@@ -18,10 +18,20 @@
 #include <linux/mutex.h>
 #include <linux/completion.h>
 
+#include "mdp3.h"
 #include "mdp3_dma.h"
 #include "mdss_fb.h"
 #include "mdss_panel.h"
 
+#define MDP3_MAX_BUF_QUEUE 8
+
+struct mdp3_buffer_queue {
+	struct mdp3_img_data img_data[MDP3_MAX_BUF_QUEUE];
+	int count;
+	int push_idx;
+	int pop_idx;
+};
+
 struct mdp3_session_data {
 	struct mutex lock;
 	int status;
@@ -32,6 +42,9 @@ struct mdp3_session_data {
 	ktime_t vsync_time;
 	spinlock_t vsync_lock;
 	struct completion vsync_comp;
+	struct mdp_overlay overlay;
+	struct mdp3_buffer_queue bufq_in;
+	struct mdp3_buffer_queue bufq_out;
 };
 
 int mdp3_ctrl_init(struct msm_fb_data_type *mfd);