[PATCH 7/8] cpufreq: mediatek: add opp notification for SVS support

Andrew-sh.Cheng
2019-05-16T09:08:44Z

From: "Andrew-sh.Cheng" <andrew-sh.cheng@mediatek.com>

cpufreq should listen opp notification and do proper actions
when receiving disable and voltage adjustment events,
which are triggered when SVS is enabled.

Signed-off-by: Andrew-sh.Cheng <andrew-sh.cheng@mediatek.com>
---
drivers/cpufreq/mediatek-cpufreq.c | 72 ++++++++++++++++++++++++++++++++++++++
1 file changed, 72 insertions(+)

diff --git a/drivers/cpufreq/mediatek-cpufreq.c b/drivers/cpufreq/mediatek-cpufreq.c
index 022d86757327..6514187a43f4 100644
--- a/drivers/cpufreq/mediatek-cpufreq.c
+++ b/drivers/cpufreq/mediatek-cpufreq.c
@@ -50,6 +50,10 @@ struct mtk_cpu_dvfs_info {
struct list_head list_head;
int intermediate_voltage;
bool need_voltage_tracking;
+ struct mutex lock; /* avoid notify and policy race condition */
+ struct notifier_block opp_nb;
+ int opp_cpu;
+ unsigned long opp_freq;
};

static LIST_HEAD(dvfs_info_list);
@@ -239,6 +243,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
vproc = dev_pm_opp_get_voltage(opp);
dev_pm_opp_put(opp);

+ mutex_lock(&info->lock);
/*
* If the new voltage or the intermediate voltage is higher than the
* current voltage, scale up voltage first.
@@ -250,6 +255,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
pr_err("cpu%d: failed to scale up voltage!\n",
policy->cpu);
mtk_cpufreq_set_voltage(info, old_vproc);
+ mutex_unlock(&info->lock);
return ret;
}
}
@@ -261,6 +267,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
policy->cpu);
mtk_cpufreq_set_voltage(info, old_vproc);
WARN_ON(1);
+ mutex_unlock(&info->lock);
return ret;
}

@@ -271,6 +278,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
policy->cpu);
clk_set_parent(cpu_clk, armpll);
mtk_cpufreq_set_voltage(info, old_vproc);
+ mutex_unlock(&info->lock);
return ret;
}

@@ -281,6 +289,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
policy->cpu);
mtk_cpufreq_set_voltage(info, inter_vproc);
WARN_ON(1);
+ mutex_unlock(&info->lock);
return ret;
}

@@ -296,15 +305,69 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
clk_set_parent(cpu_clk, info->inter_clk);
clk_set_rate(armpll, old_freq_hz);
clk_set_parent(cpu_clk, armpll);
+ mutex_unlock(&info->lock);
return ret;
}
}

+ info->opp_freq = freq_hz;
+ mutex_unlock(&info->lock);
+
return 0;
}

#define DYNAMIC_POWER "dynamic-power-coefficient"

+static int mtk_cpufreq_opp_notifier(struct notifier_block *nb,
+ unsigned long event, void *data)
+{
+ struct dev_pm_opp *opp = data;
+ struct dev_pm_opp *opp_item;
+ struct mtk_cpu_dvfs_info *info =
+ container_of(nb, struct mtk_cpu_dvfs_info, opp_nb);
+ unsigned long freq, volt;
+ int ret = 0;
+
+ if (event == OPP_EVENT_ADJUST_VOLTAGE) {
+ freq = dev_pm_opp_get_freq(opp);
+
+ mutex_lock(&info->lock);
+ if (info->opp_freq == freq) {
+ volt = dev_pm_opp_get_voltage(opp);
+ ret = mtk_cpufreq_set_voltage(info, volt);
+ if (ret)
+ dev_err(info->cpu_dev, "failed to scale voltage: %d\n",
+ ret);
+ }
+ mutex_unlock(&info->lock);
+ } else if (event == OPP_EVENT_DISABLE) {
+ freq = info->opp_freq;
+ opp_item = dev_pm_opp_find_freq_ceil(info->cpu_dev, &freq);
+ if (!IS_ERR(opp_item))
+ dev_pm_opp_put(opp_item);
+ else
+ freq = 0;
+
+ /* case of current opp is disabled */
+ if (freq == 0 || freq != info->opp_freq) {
+ // find an enable opp item
+ freq = 1;
+ opp_item = dev_pm_opp_find_freq_ceil(info->cpu_dev,
+ &freq);
+ if (!IS_ERR(opp_item)) {
+ dev_pm_opp_put(opp_item);
+ cpufreq_driver_target(
+ cpufreq_cpu_get(info->opp_cpu),
+ freq / 1000, CPUFREQ_RELATION_L);
+ } else
+ pr_err("%s: all opp items are disabled\n",
+ __func__);
+ }
+ }
+
+ return notifier_from_errno(ret);
+}
+
static int mtk_cpu_dvfs_info_init(struct mtk_cpu_dvfs_info *info, int cpu)
{
struct device *cpu_dev;
@@ -391,11 +454,20 @@ static int mtk_cpu_dvfs_info_init(struct mtk_cpu_dvfs_info *info, int cpu)
info->intermediate_voltage = dev_pm_opp_get_voltage(opp);
dev_pm_opp_put(opp);

+ info->opp_cpu = cpu;
+ info->opp_nb.notifier_call = mtk_cpufreq_opp_notifier;
+ ret = dev_pm_opp_register_notifier(cpu_dev, &info->opp_nb);
+ if (ret) {
+ pr_warn("cannot register opp notification\n");
+ goto out_free_opp_table;
+ }
+
info->cpu_dev = cpu_dev;
info->proc_reg = proc_reg;
info->sram_reg = IS_ERR(sram_reg) ? NULL : sram_reg;
info->cpu_clk = cpu_clk;
info->inter_clk = inter_clk;
+ info->opp_freq = clk_get_rate(cpu_clk);

/*
* If SRAM regulator is present, software "voltage tracking" is needed
--
2.12.5