@@ -41,6 +41,11 @@ struct mtk_cpu_dvfs_info {
int intermediate_voltage;
bool need_voltage_tracking;
int pre_vproc;
+ /* Avoid race condition for regulators between notify and policy */
+ struct mutex reg_lock;
+ struct notifier_block opp_nb;
+ int opp_cpu;
+ unsigned long opp_freq;
};
static LIST_HEAD(dvfs_info_list);
@@ -220,6 +225,9 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
inter_vproc = info->intermediate_voltage;
pre_freq_hz = clk_get_rate(cpu_clk);
+
+ mutex_lock(&info->reg_lock);
+
pre_vproc = info->pre_vproc;
if (pre_vproc <= 0)
pre_vproc = regulator_get_voltage(info->proc_reg);
@@ -250,7 +258,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
dev_err(cpu_dev,
"cpu%d: failed to scale up voltage!\n", policy->cpu);
mtk_cpufreq_set_voltage(info, pre_vproc);
- return ret;
+ goto out;
}
}
@@ -260,8 +268,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
dev_err(cpu_dev,
"cpu%d: failed to re-parent cpu clock!\n", policy->cpu);
mtk_cpufreq_set_voltage(info, pre_vproc);
- WARN_ON(1);
- return ret;
+ goto out;
}
/* Set the original PLL to target rate. */
@@ -271,7 +278,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
"cpu%d: failed to scale cpu clock rate!\n", policy->cpu);
clk_set_parent(cpu_clk, armpll);
mtk_cpufreq_set_voltage(info, pre_vproc);
- return ret;
+ goto out;
}
/* Set parent of CPU clock back to the original PLL. */
@@ -280,8 +287,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
dev_err(cpu_dev,
"cpu%d: failed to re-parent cpu clock!\n", policy->cpu);
mtk_cpufreq_set_voltage(info, inter_vproc);
- WARN_ON(1);
- return ret;
+ goto out;
}
/*
@@ -296,15 +302,72 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
clk_set_parent(cpu_clk, info->inter_clk);
clk_set_rate(armpll, pre_freq_hz);
clk_set_parent(cpu_clk, armpll);
- return ret;
+ goto out;
}
}
- return 0;
+ info->opp_freq = freq_hz;
+
+out:
+ mutex_unlock(&info->reg_lock);
+
+ return ret;
}
#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 *new_opp;
+ struct mtk_cpu_dvfs_info *info;
+ unsigned long freq, volt;
+ struct cpufreq_policy *policy;
+ int ret = 0;
+
+ info = container_of(nb, struct mtk_cpu_dvfs_info, opp_nb);
+
+ if (event == OPP_EVENT_ADJUST_VOLTAGE) {
+ freq = dev_pm_opp_get_freq(opp);
+
+ mutex_lock(&info->reg_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->reg_lock);
+ } else if (event == OPP_EVENT_DISABLE) {
+ freq = dev_pm_opp_get_freq(opp);
+
+ /* case of current opp item is disabled */
+ if (info->opp_freq == freq) {
+ freq = 1;
+ new_opp = dev_pm_opp_find_freq_ceil(info->cpu_dev,
+ &freq);
+ if (IS_ERR(new_opp)) {
+ dev_err(info->cpu_dev,
+ "all opp items are disabled\n");
+ ret = PTR_ERR(new_opp);
+ return notifier_from_errno(ret);
+ }
+
+ dev_pm_opp_put(new_opp);
+ policy = cpufreq_cpu_get(info->opp_cpu);
+ if (policy) {
+ cpufreq_driver_target(policy, freq / 1000,
+ CPUFREQ_RELATION_L);
+ cpufreq_cpu_put(policy);
+ }
+ }
+ }
+
+ return notifier_from_errno(ret);
+}
+
static int mtk_cpu_dvfs_info_init(struct mtk_cpu_dvfs_info *info, int cpu)
{
struct device *cpu_dev;
@@ -393,6 +456,17 @@ 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) {
+ dev_err(cpu_dev, "cpu%d: failed to register opp notifier\n", cpu);
+ goto out_disable_inter_clock;
+ }
+
+ mutex_init(&info->reg_lock);
+ info->opp_freq = clk_get_rate(info->cpu_clk);
+
/*
* If SRAM regulator is present, software "voltage tracking" is needed
* for this CPU power domain.
@@ -448,6 +522,9 @@ static void mtk_cpu_dvfs_info_release(struct mtk_cpu_dvfs_info *info)
}
dev_pm_opp_of_cpumask_remove_table(&info->cpus);
+
+ if (!IS_ERR_OR_NULL(info->cpu_dev))
+ dev_pm_opp_unregister_notifier(info->cpu_dev, &info->opp_nb);
}
static int mtk_cpufreq_init(struct cpufreq_policy *policy)