cpufreq-dt: Handle OPP voltage adjust events

On some SoCs the Adaptive Voltage Scaling (AVS) technique is
employed to optimize the operating voltage of a device. At a
given frequency, the hardware monitors dynamic factors and either
makes a suggestion for how much to adjust a voltage for the
current frequency, or it automatically adjusts the voltage
without software intervention.

In the former case, an AVS driver will call
dev_pm_opp_modify_voltage() and update the voltage for the
particular OPP the CPUs are using. Add an OPP notifier to
cpufreq-dt so that we can adjust the voltage of the CPU when AVS
updates the OPP.

Signed-off-by: Stephen Boyd <sboyd@codeaurora.org>

diff --git a/drivers/cpufreq/cpufreq-dt.c b/drivers/cpufreq/cpufreq-dt.c
index 90d64081ddb34ee8ba7a06372a269defdcf07a97..5b06bc34ed0ccf9066917c62ec59cca23e8d13ea 100644 (file)
--- a/drivers/cpufreq/cpufreq-dt.c
+++ b/drivers/cpufreq/cpufreq-dt.c
@@ -34,6 +34,9 @@ struct private_data {
        struct regulator *cpu_reg;
        struct thermal_cooling_device *cdev;
        unsigned int voltage_tolerance; /* in percentage */
+       struct notifier_block opp_nb;
+       struct mutex lock;
+       unsigned long opp_freq;
 };
 
 static struct freq_attr *cpufreq_dt_attr[] = {
@@ -42,6 +45,42 @@ static struct freq_attr *cpufreq_dt_attr[] = {
        NULL,
 };
 
+static int opp_notifier(struct notifier_block *nb, unsigned long event,
+                       void *data)
+{
+       struct dev_pm_opp *opp = data;
+       struct private_data *priv = container_of(nb, struct private_data,
+                                                opp_nb);
+       struct device *cpu_dev = priv->cpu_dev;
+       struct regulator *cpu_reg = priv->cpu_reg;
+       unsigned long volt, tol, freq;
+       int ret = 0;
+
+       switch (event) {
+               case OPP_EVENT_ADJUST_VOLTAGE:
+                       volt = dev_pm_opp_get_voltage(opp);
+                       freq = dev_pm_opp_get_freq(opp);
+                       tol = volt * priv->voltage_tolerance / 100;
+
+                       mutex_lock(&priv->lock);
+                       if (freq == priv->opp_freq)
+                               ret = regulator_set_voltage_tol(cpu_reg, volt,
+                                                               tol);
+                       mutex_unlock(&priv->lock);
+                       if (ret) {
+                               dev_err(cpu_dev,
+                                       "failed to scale voltage up: %d\n",
+                                       ret);
+                               return ret;
+                       }
+                       break;
+               default:
+                       break;
+       }
+
+       return 0;
+}
+
 static int set_target(struct cpufreq_policy *policy, unsigned int index)
 {
        struct dev_pm_opp *opp;
@@ -53,6 +92,7 @@ static int set_target(struct cpufreq_policy *policy, unsigned int index)
        unsigned long volt = 0, volt_old = 0, tol = 0;
        unsigned int old_freq, new_freq;
        long freq_Hz, freq_exact;
+       unsigned long opp_freq = 0;
        int ret;
 
        freq_Hz = clk_round_rate(cpu_clk, freq_table[index].frequency * 1000);
@@ -63,8 +103,8 @@ static int set_target(struct cpufreq_policy *policy, unsigned int index)
        new_freq = freq_Hz / 1000;
        old_freq = clk_get_rate(cpu_clk) / 1000;
 
+       mutex_lock(&priv->lock);
        if (!IS_ERR(cpu_reg)) {
-               unsigned long opp_freq;
 
                rcu_read_lock();
                opp = dev_pm_opp_find_freq_ceil(cpu_dev, &freq_Hz);
@@ -72,7 +112,8 @@ static int set_target(struct cpufreq_policy *policy, unsigned int index)
                        rcu_read_unlock();
                        dev_err(cpu_dev, "failed to find OPP for %ld\n",
                                freq_Hz);
-                       return PTR_ERR(opp);
+                       ret = PTR_ERR(opp);
+                       goto out;
                }
                volt = dev_pm_opp_get_voltage(opp);
                opp_freq = dev_pm_opp_get_freq(opp);
@@ -93,7 +134,7 @@ static int set_target(struct cpufreq_policy *policy, unsigned int index)
                if (ret) {
                        dev_err(cpu_dev, "failed to scale voltage up: %d\n",
                                ret);
-                       return ret;
+                       goto out;
                }
        }
 
@@ -102,7 +143,7 @@ static int set_target(struct cpufreq_policy *policy, unsigned int index)
                dev_err(cpu_dev, "failed to set clock rate: %d\n", ret);
                if (!IS_ERR(cpu_reg) && volt_old > 0)
                        regulator_set_voltage_tol(cpu_reg, volt_old, tol);
-               return ret;
+               goto out;
        }
 
        /* scaling down?  scale voltage after frequency */
@@ -112,9 +153,12 @@ static int set_target(struct cpufreq_policy *policy, unsigned int index)
                        dev_err(cpu_dev, "failed to scale voltage down: %d\n",
                                ret);
                        clk_set_rate(cpu_clk, old_freq * 1000);
+                       goto out;
                }
        }
-
+       priv->opp_freq = opp_freq;
+out:
+       mutex_unlock(&priv->lock);
        return ret;
 }
 
@@ -201,6 +245,7 @@ static int cpufreq_init(struct cpufreq_policy *policy)
        unsigned int transition_latency;
        bool need_update = false;
        int ret;
+       struct srcu_notifier_head *opp_srcu_head;
 
        ret = allocate_resources(policy->cpu, &cpu_dev, &cpu_reg, &cpu_clk);
        if (ret) {
@@ -277,6 +322,19 @@ static int cpufreq_init(struct cpufreq_policy *policy)
                goto out_free_opp;
        }
 
+       mutex_init(&priv->lock);
+
+       opp_srcu_head = dev_pm_opp_get_notifier(cpu_dev);
+       if (IS_ERR(opp_srcu_head)) {
+               ret = PTR_ERR(opp_srcu_head);
+               goto out_free_priv;
+       }
+
+       priv->opp_nb.notifier_call = opp_notifier;
+       ret = srcu_notifier_chain_register(opp_srcu_head, &priv->opp_nb);
+       if (ret)
+               goto out_free_priv;
+
        of_property_read_u32(np, "voltage-tolerance", &priv->voltage_tolerance);
 
        if (!transition_latency)
@@ -326,7 +384,7 @@ static int cpufreq_init(struct cpufreq_policy *policy)
        ret = dev_pm_opp_init_cpufreq_table(cpu_dev, &freq_table);
        if (ret) {
                pr_err("failed to init cpufreq table: %d\n", ret);
-               goto out_free_priv;
+               goto out_unregister_nb;
        }
 
        priv->cpu_dev = cpu_dev;
@@ -365,6 +423,8 @@ static int cpufreq_init(struct cpufreq_policy *policy)
 
 out_free_cpufreq_table:
        dev_pm_opp_free_cpufreq_table(cpu_dev, &freq_table);
+out_unregister_nb:
+       srcu_notifier_chain_unregister(opp_srcu_head, &priv->opp_nb);
 out_free_priv:
        kfree(priv);
 out_free_opp: