[PATCH 2/3] cpufreq: Implement freq-constraint callback

[Viresh Kumar <viresh.kumar@linaro.org>]

This implements the frequency constraint callback and registers it with
the freq-constraint framework whenever a policy is created. On policy
removal the callback is unregistered.

The constraints are also taken into consideration in
cpufreq_set_policy().

No constraints are added until now though.

Signed-off-by: Viresh Kumar <viresh.kumar@linaro.org>
---
 drivers/cpufreq/Kconfig   |  1 +
 drivers/cpufreq/cpufreq.c | 44 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 45 insertions(+)

diff --git a/drivers/cpufreq/Kconfig b/drivers/cpufreq/Kconfig
index 608af20a3494..2c2842cf2734 100644
--- a/drivers/cpufreq/Kconfig
+++ b/drivers/cpufreq/Kconfig
@@ -2,6 +2,7 @@ menu "CPU Frequency scaling"
 
 config CPU_FREQ
 	bool "CPU Frequency scaling"
+	select DEVICE_FREQ_CONSTRAINT
 	select SRCU
 	help
 	  CPU Frequency scaling allows you to change the clock speed of 
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index a8fa684f5f90..63028612d011 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -21,6 +21,7 @@
 #include <linux/cpufreq.h>
 #include <linux/delay.h>
 #include <linux/device.h>
+#include <linux/freq_constraint.h>
 #include <linux/init.h>
 #include <linux/kernel_stat.h>
 #include <linux/module.h>
@@ -1163,6 +1164,7 @@ static void cpufreq_policy_free(struct cpufreq_policy *policy)
 		per_cpu(cpufreq_cpu_data, cpu) = NULL;
 	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
 
+	freq_constraint_remove_cpumask_callback(policy->related_cpus);
 	cpufreq_policy_put_kobj(policy);
 	free_cpumask_var(policy->real_cpus);
 	free_cpumask_var(policy->related_cpus);
@@ -1170,6 +1172,24 @@ static void cpufreq_policy_free(struct cpufreq_policy *policy)
 	kfree(policy);
 }
 
+static void freq_constraint_callback(void *param)
+{
+	struct cpufreq_policy *policy = param;
+	struct cpufreq_policy new_policy = *policy;
+
+	new_policy.min = policy->user_policy.min;
+	new_policy.max = policy->user_policy.max;
+
+	down_write(&policy->rwsem);
+	if (policy_is_inactive(policy))
+		goto unlock;
+
+	cpufreq_set_policy(policy, &new_policy);
+
+unlock:
+	up_write(&policy->rwsem);
+}
+
 static int cpufreq_online(unsigned int cpu)
 {
 	struct cpufreq_policy *policy;
@@ -1236,6 +1256,14 @@ static int cpufreq_online(unsigned int cpu)
 			per_cpu(cpufreq_cpu_data, j) = policy;
 			add_cpu_dev_symlink(policy, j);
 		}
+
+		ret = freq_constraint_set_cpumask_callback(policy->related_cpus,
+					freq_constraint_callback, policy);
+		if (ret) {
+			pr_err("Failed to set freq-constraints: %d (%*pbl)\n",
+			       ret, cpumask_pr_args(policy->cpus));
+			goto out_destroy_policy;
+		}
 	} else {
 		policy->min = policy->user_policy.min;
 		policy->max = policy->user_policy.max;
@@ -2198,6 +2226,8 @@ static int cpufreq_set_policy(struct cpufreq_policy *policy,
 				struct cpufreq_policy *new_policy)
 {
 	struct cpufreq_governor *old_gov;
+	struct device *cpu_dev = get_cpu_device(policy->cpu);
+	unsigned long fc_min, fc_max;
 	int ret;
 
 	pr_debug("setting new policy for CPU %u: %u - %u kHz\n",
@@ -2217,6 +2247,20 @@ static int cpufreq_set_policy(struct cpufreq_policy *policy,
 	if (ret)
 		return ret;
 
+	ret = freq_constraints_get(cpu_dev, &fc_min, &fc_max);
+	if (ret) {
+		dev_err(cpu_dev, "cpufreq: Failed to get freq-constraints\n");
+	} else {
+		if (fc_min > new_policy->min)
+			new_policy->min = fc_min;
+		if (fc_max < new_policy->max)
+			new_policy->max = fc_max;
+	}
+
+	/*
+	 * The notifier-chain shall be removed once all the users of
+	 * CPUFREQ_ADJUST are moved to use the freq-constraints.
+	 */
 	/* adjust if necessary - all reasons */
 	blocking_notifier_call_chain(&cpufreq_policy_notifier_list,
 			CPUFREQ_ADJUST, new_policy);
-- 
2.7.4