From 4b5e72686977d7080829f88b2ded37bb51e27ef3 Mon Sep 17 00:00:00 2001
From: Rama Aparna Mallavarapu <aparnam@codeaurora.org>
Date: Wed, 3 Apr 2019 12:12:30 -0700
Subject: [PATCH] devfreq: bimc_bwmon: Add support to enable BWMON clks

Some BWMON devices requires clocks to be enabled, hence
add necessary support in the bwmon driver to enable the
required clocks if any.

Change-Id: Ie06731f764ff24d602ae2a86dea4f39ce75df800
Signed-off-by: Rama Aparna Mallavarapu <aparnam@codeaurora.org>
[avajid@codeaurora.org: made minor styling changes]
Signed-off-by: Amir Vajid <avajid@codeaurora.org>
---
 drivers/devfreq/bimc-bwmon.c | 82 ++++++++++++++++++++++++++++++++++++
 1 file changed, 82 insertions(+)

diff --git a/drivers/devfreq/bimc-bwmon.c b/drivers/devfreq/bimc-bwmon.c
index 1c8c47b56ecc..dbb046630342 100644
--- a/drivers/devfreq/bimc-bwmon.c
+++ b/drivers/devfreq/bimc-bwmon.c
@@ -20,6 +20,7 @@
 #include <linux/spinlock.h>
 #include <linux/log2.h>
 #include <linux/sizes.h>
+#include <linux/clk.h>
 #include "governor_bw_hwmon.h"
 
 #define GLB_INT_STATUS(m)	((m)->global_base + 0x100)
@@ -91,6 +92,8 @@ struct bwmon {
 	void __iomem		*global_base;
 	unsigned int		mport;
 	int			irq;
+	int			nr_clks;
+	struct clk		**clks;
 	const struct bwmon_spec	*spec;
 	struct device		*dev;
 	struct bw_hwmon		hw;
@@ -775,6 +778,27 @@ void mon_set_byte_count_filter(struct bwmon *m, enum mon_reg_type type)
 	}
 }
 
+static __always_inline int mon_clk_enable(struct bwmon *m)
+{
+	int ret;
+	int i;
+
+	for (i = 0; i < m->nr_clks; i++) {
+		ret = clk_prepare_enable(m->clks[i]);
+		if (ret < 0) {
+			dev_err(m->dev, "BWMON clk not enabled: %d\n", ret);
+			goto err;
+		}
+	}
+
+	return 0;
+err:
+	for (i--; i >= 0; i--)
+		clk_disable_unprepare(m->clks[i]);
+
+	return ret;
+}
+
 static __always_inline int __start_bw_hwmon(struct bw_hwmon *hw,
 		unsigned long mbps, enum mon_reg_type type)
 {
@@ -783,6 +807,12 @@ static __always_inline int __start_bw_hwmon(struct bw_hwmon *hw,
 	int ret;
 	irq_handler_t handler;
 
+	ret = mon_clk_enable(m);
+	if (ret < 0) {
+		dev_err(m->dev, "Unable to turn on bwmon clks! (%d)\n", ret);
+		return ret;
+	}
+
 	switch (type) {
 	case MON1:
 		handler = bwmon_intr_handler;
@@ -849,6 +879,14 @@ static int start_bw_hwmon3(struct bw_hwmon *hw, unsigned long mbps)
 	return __start_bw_hwmon(hw, mbps, MON3);
 }
 
+static __always_inline void mon_clk_disable(struct bwmon *m)
+{
+	int i;
+
+	for (i = m->nr_clks - 1; i >= 0; i--)
+		clk_disable_unprepare(m->clks[i]);
+}
+
 static __always_inline
 void __stop_bw_hwmon(struct bw_hwmon *hw, enum mon_reg_type type)
 {
@@ -859,6 +897,7 @@ void __stop_bw_hwmon(struct bw_hwmon *hw, enum mon_reg_type type)
 	mon_disable(m, type);
 	mon_clear(m, true, type);
 	mon_irq_clear(m, type);
+	mon_clk_disable(m);
 }
 
 static void stop_bw_hwmon(struct bw_hwmon *hw)
@@ -911,6 +950,12 @@ int __resume_bw_hwmon(struct bw_hwmon *hw, enum mon_reg_type type)
 	int ret;
 	irq_handler_t handler;
 
+	ret = mon_clk_enable(m);
+	if (ret < 0) {
+		dev_err(m->dev, "Unable to turn on bwmon clks! (%d)\n", ret);
+		return ret;
+	}
+
 	switch (type) {
 	case MON1:
 		handler = bwmon_intr_handler;
@@ -1014,6 +1059,7 @@ static int bimc_bwmon_driver_probe(struct platform_device *pdev)
 	struct bwmon *m;
 	int ret;
 	u32 data, count_unit;
+	unsigned int len, i;
 
 	m = devm_kzalloc(dev, sizeof(*m), GFP_KERNEL);
 	if (!m)
@@ -1059,6 +1105,42 @@ static int bimc_bwmon_driver_probe(struct platform_device *pdev)
 		m->mport = data;
 	}
 
+	if (of_find_property(dev->of_node, "qcom,bwmon_clks", &len)) {
+		m->nr_clks = of_property_count_strings(dev->of_node,
+							"qcom,bwmon_clks");
+		if (!m->nr_clks) {
+			dev_err(dev, "Failed to get clock names\n");
+			return -EINVAL;
+		}
+
+		m->clks = devm_kzalloc(dev, sizeof(struct clk *) * m->nr_clks,
+					GFP_KERNEL);
+		if (!m->clks)
+			return -ENOMEM;
+
+		for (i = 0; i < m->nr_clks; i++) {
+			const char *clock_name;
+
+			ret = of_property_read_string_index(dev->of_node,
+						"qcom,bwmon_clks", i,
+							&clock_name);
+			if (ret < 0) {
+				pr_err("failed to read clk index %d ret %d\n",
+									i, ret);
+				return ret;
+			}
+			m->clks[i] = devm_clk_get(dev, clock_name);
+			if (IS_ERR(m->clks[i])) {
+				ret = PTR_ERR(m->clks[i]);
+				if (ret != -EPROBE_DEFER)
+					dev_err(dev, "Error to get %s clk %d\n",
+							clock_name, ret);
+				return ret;
+			}
+		}
+	} else
+		m->nr_clks = 0;
+
 	m->irq = platform_get_irq(pdev, 0);
 	if (m->irq < 0) {
 		dev_err(dev, "Unable to get IRQ number\n");