diff --git a/drivers/devfreq/bimc-bwmon.c b/drivers/devfreq/bimc-bwmon.c index 1c9f77fac738..af19be8c6628 100644 --- a/drivers/devfreq/bimc-bwmon.c +++ b/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 #include #include +#include #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");