msm: mdss: Allow for multiple vsync handlers

Allow for multiple vsync handlers to be registered dynamically. Allows for
per handler reference counts in case of multiple drivers being dependent on
a single handler.

Change-Id: I92b25f2974d6c9978cfa1abdd27d8c951c5ad52d
Signed-off-by: Carl Vanderlip <carlv@codeaurora.org>

5 files changed, 91 insertions, 27 deletions

diff --git a/drivers/video/fbdev/msm/mdss_mdp.h b/drivers/video/fbdev/msm/mdss_mdp.h
index a531fcd46a27..e58ff9148c90 100644
--- a/drivers/video/fbdev/msm/mdss_mdp.h
+++ b/drivers/video/fbdev/msm/mdss_mdp.h
@@ -109,6 +109,12 @@ enum mdss_mdp_csc_type {
 struct mdss_mdp_ctl;
 typedef void (*mdp_vsync_handler_t)(struct mdss_mdp_ctl *, ktime_t);
 
+struct mdss_mdp_vsync_handler {
+	mdp_vsync_handler_t vsync_handler;
+	u32 ref_cnt;
+	struct list_head list;
+};
+
 struct mdss_mdp_ctl {
 	u32 num;
 	char __iomem *base;
@@ -144,14 +150,18 @@ struct mdss_mdp_ctl {
 	struct mutex lock;
 
 	struct mdss_panel_data *panel_data;
+	struct mdss_mdp_vsync_handler vsync_handler;
 
 	int (*start_fnc) (struct mdss_mdp_ctl *ctl);
 	int (*stop_fnc) (struct mdss_mdp_ctl *ctl);
 	int (*prepare_fnc) (struct mdss_mdp_ctl *ctl, void *arg);
 	int (*display_fnc) (struct mdss_mdp_ctl *ctl, void *arg);
 	int (*wait_fnc) (struct mdss_mdp_ctl *ctl, void *arg);
-	int (*set_vsync_handler) (struct mdss_mdp_ctl *, mdp_vsync_handler_t);
 	u32 (*read_line_cnt_fnc) (struct mdss_mdp_ctl *);
+	int (*add_vsync_handler) (struct mdss_mdp_ctl *,
+					struct mdss_mdp_vsync_handler *);
+	int (*remove_vsync_handler) (struct mdss_mdp_ctl *,
+					struct mdss_mdp_vsync_handler *);
 
 	void *priv_data;
 };
diff --git a/drivers/video/fbdev/msm/mdss_mdp_ctl.c b/drivers/video/fbdev/msm/mdss_mdp_ctl.c
index 6c9cce2bc716..ee27fb189bd0 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_ctl.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_ctl.c
@@ -300,8 +300,9 @@ static int mdss_mdp_ctl_free(struct mdss_mdp_ctl *ctl)
 	ctl->prepare_fnc = NULL;
 	ctl->display_fnc = NULL;
 	ctl->wait_fnc = NULL;
-	ctl->set_vsync_handler = NULL;
 	ctl->read_line_cnt_fnc = NULL;
+	ctl->add_vsync_handler = NULL;
+	ctl->remove_vsync_handler = NULL;
 	mutex_unlock(&mdss_mdp_ctl_lock);
 
 	return 0;
diff --git a/drivers/video/fbdev/msm/mdss_mdp_intf_cmd.c b/drivers/video/fbdev/msm/mdss_mdp_intf_cmd.c
index e0be8622fb44..eb2bd21fe1e9 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_intf_cmd.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_intf_cmd.c
@@ -235,7 +235,7 @@ static void clk_ctrl_work(struct work_struct *work)
 }
 
 static int mdss_mdp_cmd_vsync_ctrl(struct mdss_mdp_ctl *ctl,
-		mdp_vsync_handler_t send_vsync)
+		struct mdss_mdp_vsync_handler *handler)
 {
 	struct mdss_mdp_cmd_ctx *ctx;
 	unsigned long flags;
@@ -247,7 +247,7 @@ static int mdss_mdp_cmd_vsync_ctrl(struct mdss_mdp_ctl *ctl,
 		return -ENODEV;
 	}
 
-	enable = (send_vsync != NULL);
+	enable = (handler->vsync_handler != NULL);
 
 	pr_debug("%s: ctx=%p ctx=%d enabled=%d %d clk_enabled=%d clk_ctrl=%d\n",
 			__func__, ctx, ctx->pp_num, ctx->vsync_enabled, enable,
@@ -263,7 +263,7 @@ static int mdss_mdp_cmd_vsync_ctrl(struct mdss_mdp_ctl *ctl,
 		spin_lock_irqsave(&ctx->clk_lock, flags);
 		ctx->clk_control = 0;
 		ctx->expire = 0;
-		ctx->send_vsync = send_vsync;
+		ctx->send_vsync = handler->vsync_handler;
 		spin_unlock_irqrestore(&ctx->clk_lock, flags);
 		if (ctx->clk_enabled == 0) {
 			mdss_mdp_clk_ctrl(MDP_BLOCK_POWER_ON, false);
@@ -376,6 +376,7 @@ int mdss_mdp_cmd_stop(struct mdss_mdp_ctl *ctl)
 {
 	struct mdss_mdp_cmd_ctx *ctx;
 	int need_wait = 0;
+	struct mdss_mdp_vsync_handler null_handle;
 	int ret;
 
 	ctx = (struct mdss_mdp_cmd_ctx *) ctl->priv_data;
@@ -399,7 +400,8 @@ int mdss_mdp_cmd_stop(struct mdss_mdp_ctl *ctl)
 
 	ctx->panel_on = 0;
 
-	mdss_mdp_cmd_vsync_ctrl(ctl, NULL);
+	null_handle.vsync_handler = NULL;
+	mdss_mdp_cmd_vsync_ctrl(ctl, &null_handle);
 
 	mdss_mdp_set_intr_callback(MDSS_MDP_IRQ_PING_PONG_RD_PTR, ctl->intf_num,
 				   NULL, NULL);
@@ -478,7 +480,8 @@ int mdss_mdp_cmd_start(struct mdss_mdp_ctl *ctl)
 	ctl->stop_fnc = mdss_mdp_cmd_stop;
 	ctl->display_fnc = mdss_mdp_cmd_kickoff;
 	ctl->wait_fnc = mdss_mdp_cmd_wait4comp;
-	ctl->set_vsync_handler = mdss_mdp_cmd_vsync_ctrl;
+	ctl->add_vsync_handler = mdss_mdp_cmd_vsync_ctrl;
+	ctl->remove_vsync_handler = mdss_mdp_cmd_vsync_ctrl;
 
 	pr_debug("%s:-\n", __func__);
 
diff --git a/drivers/video/fbdev/msm/mdss_mdp_intf_video.c b/drivers/video/fbdev/msm/mdss_mdp_intf_video.c
index 94ae710365d2..f77bd0141517 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_intf_video.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_intf_video.c
@@ -50,7 +50,7 @@ struct mdss_mdp_video_ctx {
 
 	atomic_t vsync_ref;
 	spinlock_t vsync_lock;
-	mdp_vsync_handler_t vsync_handler;
+	struct list_head vsync_handlers;
 };
 
 static inline void mdp_video_write(struct mdss_mdp_video_ctx *ctx,
@@ -92,6 +92,7 @@ int mdss_mdp_video_addr_setup(struct mdss_data_type *mdata,
 				offsets[i], head[i].base);
 		head[i].ref_cnt = 0;
 		head[i].intf_num = i + MDSS_MDP_INTF0;
+		INIT_LIST_HEAD(&head[i].vsync_handlers);
 	}
 
 	mdata->video_intf = head;
@@ -212,32 +213,76 @@ static inline void video_vsync_irq_disable(struct mdss_mdp_ctl *ctl)
 		mdss_mdp_irq_disable(MDSS_MDP_IRQ_INTF_VSYNC, ctl->intf_num);
 }
 
-static int mdss_mdp_video_set_vsync_handler(struct mdss_mdp_ctl *ctl,
-		mdp_vsync_handler_t vsync_handler)
+static int mdss_mdp_video_add_vsync_handler(struct mdss_mdp_ctl *ctl,
+		struct mdss_mdp_vsync_handler *handle)
 {
 	struct mdss_mdp_video_ctx *ctx;
 	unsigned long flags;
-	int need_update;
+	struct mdss_mdp_vsync_handler *tmp;
+	bool exist = false;
+	int ret = 0;
+
+	if (!handle || !(handle->vsync_handler)) {
+		ret = -EINVAL;
+		goto exit;
+	}
 
 	ctx = (struct mdss_mdp_video_ctx *) ctl->priv_data;
 	if (!ctx) {
 		pr_err("invalid ctx for ctl=%d\n", ctl->num);
-		return -ENODEV;
+		ret = -ENODEV;
+		goto exit;
 	}
 
 	spin_lock_irqsave(&ctx->vsync_lock, flags);
-	need_update = (!ctx->vsync_handler && vsync_handler) ||
-			(ctx->vsync_handler && !vsync_handler);
-	ctx->vsync_handler = vsync_handler;
+	list_for_each_entry(tmp, &(ctx->vsync_handlers), list) {
+		if (tmp->vsync_handler == handle->vsync_handler) {
+			exist = true;
+			tmp->ref_cnt++;
+		}
+	}
+	if (!exist) {
+		handle->ref_cnt = 1;
+		list_add(&handle->list, &ctx->vsync_handlers);
+	}
+
+	video_vsync_irq_enable(ctl);
 	spin_unlock_irqrestore(&ctx->vsync_lock, flags);
+exit:
+	return ret;
+}
 
-	if (need_update) {
-		if (vsync_handler)
-			video_vsync_irq_enable(ctl);
-		else
-			video_vsync_irq_disable(ctl);
+/* passing NULL as handle or vsync_handler will clear all handlers */
+static int mdss_mdp_video_remove_vsync_handler(struct mdss_mdp_ctl *ctl,
+		struct mdss_mdp_vsync_handler *handle)
+{
+	struct mdss_mdp_video_ctx *ctx;
+	unsigned long flags;
+	struct mdss_mdp_vsync_handler *tmp, *q;
+	bool exist = true;
+	bool used = false;
+
+	ctx = (struct mdss_mdp_video_ctx *) ctl->priv_data;
+	if (!ctx) {
+		pr_err("invalid ctx for ctl=%d\n", ctl->num);
+		return -ENODEV;
 	}
 
+	spin_lock_irqsave(&ctx->vsync_lock, flags);
+	list_for_each_entry_safe(tmp, q, &ctx->vsync_handlers, list) {
+		if (handle && handle->vsync_handler)
+			exist = (tmp->vsync_handler == handle->vsync_handler);
+		if (exist) {
+			tmp->ref_cnt--;
+			if (handle && handle->vsync_handler)
+				used = (tmp->ref_cnt != 0);
+			if (!used) {
+				video_vsync_irq_disable(ctl);
+				list_del_init(&tmp->list);
+			}
+		}
+	}
+	spin_unlock_irqrestore(&ctx->vsync_lock, flags);
 	return 0;
 }
 
@@ -274,7 +319,7 @@ static int mdss_mdp_video_stop(struct mdss_mdp_ctl *ctl)
 			ctl->intf_num);
 	}
 
-	mdss_mdp_video_set_vsync_handler(ctl, NULL);
+	mdss_mdp_video_remove_vsync_handler(ctl, NULL);
 
 	mdss_mdp_set_intr_callback(MDSS_MDP_IRQ_INTF_VSYNC, ctl->intf_num,
 				   NULL, NULL);
@@ -291,6 +336,7 @@ static void mdss_mdp_video_vsync_intr_done(void *arg)
 {
 	struct mdss_mdp_ctl *ctl = arg;
 	struct mdss_mdp_video_ctx *ctx = ctl->priv_data;
+	struct mdss_mdp_vsync_handler *tmp;
 	ktime_t vsync_time;
 
 	if (!ctx) {
@@ -306,8 +352,9 @@ static void mdss_mdp_video_vsync_intr_done(void *arg)
 
 	complete_all(&ctx->vsync_comp);
 	spin_lock(&ctx->vsync_lock);
-	if (ctx->vsync_handler)
-		ctx->vsync_handler(ctl, vsync_time);
+	list_for_each_entry(tmp, &ctx->vsync_handlers, list) {
+		tmp->vsync_handler(ctl, vsync_time);
+	}
 	spin_unlock(&ctx->vsync_lock);
 }
 
@@ -471,8 +518,9 @@ int mdss_mdp_video_start(struct mdss_mdp_ctl *ctl)
 	ctl->stop_fnc = mdss_mdp_video_stop;
 	ctl->display_fnc = mdss_mdp_video_display;
 	ctl->wait_fnc = mdss_mdp_video_wait4comp;
-	ctl->set_vsync_handler = mdss_mdp_video_set_vsync_handler;
 	ctl->read_line_cnt_fnc = mdss_mdp_video_line_count;
+	ctl->add_vsync_handler = mdss_mdp_video_add_vsync_handler;
+	ctl->remove_vsync_handler = mdss_mdp_video_remove_vsync_handler;
 
 	return 0;
 }
diff --git a/drivers/video/fbdev/msm/mdss_mdp_overlay.c b/drivers/video/fbdev/msm/mdss_mdp_overlay.c
index 80d917c18001..a62a170a5235 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_overlay.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_overlay.c
@@ -1302,7 +1302,7 @@ int mdss_mdp_overlay_vsync_ctrl(struct msm_fb_data_type *mfd, int en)
 
 	if (!ctl)
 		return -ENODEV;
-	if (!ctl->set_vsync_handler)
+	if (!ctl->add_vsync_handler || !ctl->remove_vsync_handler)
 		return -ENOTSUPP;
 
 	rc = mutex_lock_interruptible(&ctl->lock);
@@ -1325,9 +1325,9 @@ int mdss_mdp_overlay_vsync_ctrl(struct msm_fb_data_type *mfd, int en)
 
 	mdss_mdp_clk_ctrl(MDP_BLOCK_POWER_ON, false);
 	if (en)
-		rc = ctl->set_vsync_handler(ctl, mdss_mdp_overlay_handle_vsync);
+		rc = ctl->add_vsync_handler(ctl, &ctl->vsync_handler);
 	else
-		rc = ctl->set_vsync_handler(ctl, NULL);
+		rc = ctl->remove_vsync_handler(ctl, &ctl->vsync_handler);
 	mdss_mdp_clk_ctrl(MDP_BLOCK_POWER_OFF, false);
 
 	mutex_unlock(&ctl->lock);
@@ -1875,6 +1875,8 @@ static int mdss_mdp_overlay_on(struct msm_fb_data_type *mfd)
 				mfd->index);
 			return PTR_ERR(ctl);
 		}
+		ctl->vsync_handler.vsync_handler =
+						mdss_mdp_overlay_handle_vsync;
 
 		if (mfd->split_display && pdata->next) {
 			/* enable split display */