From 1d750fe50f8cb12a7a5e5aeeea84a5bda6fbf5e0 Mon Sep 17 00:00:00 2001
From: Karthikeyan Mani <kmani@codeaurora.org>
Date: Fri, 6 Sep 2019 14:36:09 -0700
Subject: [PATCH] soc: swr-mstr: Vote for core vote before clock request

For soundwire devices, if core vote function is available,
do a core vote before doing a clock request. This way
we retain any hardware votes needed before doing a read
or write.

Change-Id: If29b5f102be4f2d60e6d71cae70ac43e3fdaa00c
Signed-off-by: Karthikeyan Mani <kmani@codeaurora.org>
---
 soc/swr-mstr-ctrl.c | 17 ++++++++++++++++-
 soc/swr-mstr-ctrl.h |  2 ++
 2 files changed, 18 insertions(+), 1 deletion(-)

diff --git a/soc/swr-mstr-ctrl.c b/soc/swr-mstr-ctrl.c
index 9ad849e866d0..bd4f56acc8e2 100644
--- a/soc/swr-mstr-ctrl.c
+++ b/soc/swr-mstr-ctrl.c
@@ -301,6 +301,15 @@ static int swrm_clk_request(struct swr_mstr_ctrl *swrm, bool enable)
 			ret = -ENODEV;
 			goto exit;
 		}
+		if (swrm->core_vote) {
+			ret = swrm->core_vote(swrm->handle, true);
+			if (ret) {
+				dev_err_ratelimited(swrm->dev,
+					"%s: clock enable req failed",
+					__func__);
+				goto exit;
+			}
+		}
 		swrm->clk_ref_count++;
 		if (swrm->clk_ref_count == 1) {
 			ret = swrm->clk(swrm->handle, true);
@@ -2123,6 +2132,7 @@ static int swrm_probe(struct platform_device *pdev)
 					swrm->swrm_base_reg, SWRM_MAX_REGISTER);
 	}
 
+	swrm->core_vote = pdata->core_vote;
 	swrm->clk = pdata->clk;
 	if (!swrm->clk) {
 		dev_err(&pdev->dev, "%s: swrm->clk is NULL\n",
@@ -2584,7 +2594,12 @@ static int swrm_runtime_suspend(struct device *dev)
 			mutex_lock(&swrm->reslock);
 			usleep_range(100, 105);
 		}
-		swrm_clk_request(swrm, false);
+		ret = swrm_clk_request(swrm, false);
+		if (ret) {
+			dev_err(dev, "%s: swrmn clk failed\n", __func__);
+			ret = 0;
+			goto exit;
+		}
 
 		if (swrm->clk_stop_mode0_supp) {
 			if (swrm->wake_irq > 0) {
diff --git a/soc/swr-mstr-ctrl.h b/soc/swr-mstr-ctrl.h
index e05ede258dc9..24bca1b047c7 100644
--- a/soc/swr-mstr-ctrl.h
+++ b/soc/swr-mstr-ctrl.h
@@ -91,6 +91,7 @@ struct swr_ctrl_platform_data {
 	int (*write)(void *handle, int reg, int val);
 	int (*bulk_write)(void *handle, u32 *reg, u32 *val, size_t len);
 	int (*clk)(void *handle, bool enable);
+	int (*core_vote)(void *handle, bool enable);
 	int (*reg_irq)(void *handle, irqreturn_t(*irq_handler)(int irq,
 			void *data), void *swr_handle, int type);
 };
@@ -120,6 +121,7 @@ struct swr_mstr_ctrl {
 	int (*write)(void *handle, int reg, int val);
 	int (*bulk_write)(void *handle, u32 *reg, u32 *val, size_t len);
 	int (*clk)(void *handle, bool enable);
+	int (*core_vote)(void *handle, bool enable);
 	int (*reg_irq)(void *handle, irqreturn_t(*irq_handler)(int irq,
 			void *data), void *swr_handle, int type);
 	int irq;