[PATCH 2/2] remoteproc: qcom: Add clock init and enable support if needed.

[Avaneesh Kumar Dwivedi <akdwived@codeaurora.org>]

To bring venus firmware module standalone on msm8996, it need
certain multimedia clocks to be enabled. Adding support for
initialization and enable disable of such clocks.

Signed-off-by: Avaneesh Kumar Dwivedi <akdwived@codeaurora.org>
---
 drivers/remoteproc/qcom_venus_pil.c | 72 +++++++++++++++++++++++++++++++++++++
 1 file changed, 72 insertions(+)

diff --git a/drivers/remoteproc/qcom_venus_pil.c b/drivers/remoteproc/qcom_venus_pil.c
index 5b4ea10..1a41a23 100644
--- a/drivers/remoteproc/qcom_venus_pil.c
+++ b/drivers/remoteproc/qcom_venus_pil.c
@@ -19,6 +19,7 @@
 #include <linux/module.h>
 #include <linux/of_reserved_mem.h>
 #include <linux/platform_device.h>
+#include <linux/clk.h>
 #include <linux/qcom_scm.h>
 #include <linux/remoteproc.h>
 #include <linux/of_device.h>
@@ -86,11 +87,47 @@ static int venus_load(struct rproc *rproc, const struct firmware *fw)
 	.load = venus_load,
 };
 
+static int qcom_venus_clk_enable(struct qcom_venus *venus)
+{
+	int i, ret;
+
+	if (!venus->venus_clks)
+		return 0;
+
+	for (i = 0; i < venus->venus_rproc_res->venus_clk_cnt; i++) {
+		ret = clk_prepare_enable(venus->venus_clks[i]);
+		if (ret) {
+			for (; i > 0; i--) {
+				clk_disable_unprepare(venus->venus_clks[i]);
+				return ret;
+			}
+		}
+	}
+	return 0;
+}
+
+static void qcom_venus_clk_disable(struct qcom_venus *venus)
+{
+	int i;
+
+	if (!venus->venus_clks)
+		return;
+
+	for (i = venus->venus_rproc_res->venus_clk_cnt-1; i >= 0; i--)
+		clk_disable_unprepare(venus->venus_clks[i]);
+}
+
 static int venus_start(struct rproc *rproc)
 {
 	struct qcom_venus *venus = rproc->priv;
 	int ret;
 
+	ret = qcom_venus_clk_enable(venus);
+	if (ret) {
+		dev_err(venus->dev, "failed to enable venus_clk\n");
+		return ret;
+	}
+
 	ret = qcom_scm_pas_auth_and_reset(VENUS_PAS_ID);
 	if (ret)
 		dev_err(venus->dev,
@@ -109,6 +146,8 @@ static int venus_stop(struct rproc *rproc)
 	if (ret)
 		dev_err(venus->dev, "failed to shutdown: %d\n", ret);
 
+	qcom_venus_clk_disable(venus);
+
 	return ret;
 }
 
@@ -131,6 +170,34 @@ static void *venus_da_to_va(struct rproc *rproc, u64 da, int len)
 	.da_to_va = venus_da_to_va,
 };
 
+static int qcom_venus_init_clocks(struct qcom_venus *venus)
+{
+	struct clk **clk_arr;
+	int i;
+
+	if (venus->venus_rproc_res->venus_clk_cnt) {
+		clk_arr = devm_kzalloc(venus->dev,
+		sizeof(clk_arr) * venus->venus_rproc_res->venus_clk_cnt,
+		GFP_KERNEL);
+
+		for (i = 0; i < venus->venus_rproc_res->venus_clk_cnt; i++) {
+			clk_arr[i] = devm_clk_get(venus->dev,
+			venus->venus_rproc_res->venus_clks[i]);
+
+			if (IS_ERR(clk_arr[i])) {
+				dev_err(venus->dev, "failed to get %s clock\n",
+				venus->venus_rproc_res->venus_clks[i]);
+				return PTR_ERR(clk_arr[i]);
+			}
+			clk_set_rate(clk_arr[i], clk_round_rate(clk_arr[i],
+				venus->venus_rproc_res->venus_clk_rate[i]));
+		}
+		venus->venus_clks = clk_arr;
+	}
+	venus->venus_clks = NULL;
+	return 0;
+}
+
 static int venus_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -172,6 +239,11 @@ static int venus_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, venus);
 
 	venus->venus_rproc_res = desc;
+
+	ret = qcom_venus_init_clocks(venus);
+	if (ret)
+		goto free_rproc;
+
 	venus->mem_va = dma_alloc_coherent(dev, venus->mem_size,
 					   &venus->mem_phys, GFP_KERNEL);
 	if (!venus->mem_va) {
-- 
Qualcomm India Private Limited, on behalf of Qualcomm Innovation Center, Inc. is a member of the Code Aurora Forum,
a Linux Foundation Collaborative Project.