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>

drivers/devfreq/bimc-bwmon.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2018, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2014-2019, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -28,6 +28,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)
@@ -99,6 +100,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;
@@ -783,6 +786,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) {
+			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)
 {
@@ -791,6 +815,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) {
+		dev_err(m->dev, "Unable to turn on bwmon clks! (%d)\n", ret);
+		return ret;
+	}
+
 	switch (type) {
 	case MON1:
 		handler = bwmon_intr_handler;
@@ -857,6 +887,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)
 {
@@ -867,6 +905,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)
@@ -919,6 +958,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) {
+		dev_err(m->dev, "Unable to turn on bwmon clks! (%d)\n", ret);
+		return ret;
+	}
+
 	switch (type) {
 	case MON1:
 		handler = bwmon_intr_handler;
@@ -1022,6 +1067,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)
@@ -1067,6 +1113,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) {
+				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");