[ARM/tegra] add cpufreq driver

tegra cpufreq uses the dynamic voltage and frequency scaling code
in the tegra RM to update the CPU and bus clocks on-the-fly, and
to dynamically add and remove CPUs from the SMP cluster. this code
runs in a background kernel task named cpufreq-dvfsd.

all of the resources needed for cpufreq are allocated during kernel
initialization; however, starting the DVFS thread is deferred until
tegra_start_dvfsd is called (triggered by a user-space action, such as
opening the RM device node, an ioctl on the RM device node, or writing
to a sysfs node), because running the DVFS daemon during kernel
init causes numerous problems

Change-Id: I113e6a71b2405e9f33b9ec4912ff40d229d96a02

1 files changed, 267 insertions, 0 deletions

diff --git a/arch/arm/mach-tegra/cpufreq.c b/arch/arm/mach-tegra/cpufreq.c
new file mode 100644
index 000000000000..dd5c45b8a77d
--- /dev/null
+++ b/arch/arm/mach-tegra/cpufreq.c
@@ -0,0 +1,267 @@
+/*
+ * arch/arm/mach-tegra/cpufreq.c
+ *
+ * cpufreq driver for NVIDIA Tegra SoCs
+ *
+ * Copyright (c) 2008-2010, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/cpufreq.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/smp.h>
+#include <linux/cpu.h>
+#include <linux/freezer.h>
+#include <linux/kthread.h>
+#include <linux/workqueue.h>
+#include <linux/smp_lock.h>
+
+#include <asm/system.h>
+#include <asm/smp_twd.h>
+
+#include <mach/hardware.h>
+#include <mach/nvrm_linux.h>
+#include <mach/timex.h>
+
+#include <nvrm_power.h>
+#include <nvrm_power_private.h>
+
+static NvRmDeviceHandle rm_cpufreq = NULL;
+static struct task_struct *cpufreq_dfsd = NULL;
+static struct clk *clk_cpu = NULL;
+
+static DEFINE_MUTEX(init_mutex);
+
+static void tegra_cpufreq_hotplug(NvRmPmRequest req)
+{
+	int rc = 0;
+#ifdef CONFIG_HOTPLUG_CPU
+	unsigned int cpu;
+
+	if (req & NvRmPmRequest_CpuOnFlag) {
+		struct cpumask m;
+
+		cpumask_andnot(&m, cpu_present_mask, cpu_online_mask);
+		cpu = cpumask_any(&m);
+
+		if (cpu_present(cpu) && !cpu_online(cpu))
+			rc = cpu_up(cpu);
+
+	} else if (req & NvRmPmRequest_CpuOffFlag) {
+		cpu = cpumask_any_but(cpu_online_mask, 0);
+
+		if (cpu_present(cpu) && cpu_online(cpu))
+			rc = cpu_down(cpu);
+	}
+#endif
+	if (rc)
+		pr_err("%s: error %d servicing hot plug request\n",
+		       __func__, rc);
+	
+}
+
+static int tegra_cpufreq_dfsd(void *arg)
+{
+	unsigned long rate, last_rate;
+	NvRmPmRequest req = 0;
+
+	BUG_ON(!clk_cpu);
+
+	rate = clk_get_rate(clk_cpu);
+	last_rate = rate;
+
+	NvRmDfsSetState(rm_cpufreq, NvRmDfsRunState_ClosedLoop);
+	set_freezable_with_signal();
+
+	while (!kthread_should_stop() && !(req & NvRmPmRequest_ExitFlag)) {
+
+		req = NvRmPrivPmThread();
+
+		if (try_to_freeze())
+			continue;
+
+		tegra_cpufreq_hotplug(req);
+
+#ifdef CONFIG_USE_ARM_TWD_PRESCALER
+		rate = clk_get_rate(clk_cpu);
+		if (rate != last_rate) {
+			local_timer_rescale(rate / 1000);
+			smp_wmb();
+			on_each_cpu(twd_set_prescaler, NULL, true);
+			last_rate = rate;
+		}
+#endif
+	}
+	pr_info("dvfs thead shutdown\n");
+
+	return 0;
+}
+
+static int tegra_verify_speed(struct cpufreq_policy *policy)
+{
+	cpufreq_verify_within_limits(policy, policy->cpuinfo.min_freq,
+		policy->cpuinfo.max_freq);
+	return 0;
+}
+
+static unsigned int tegra_get_speed(unsigned int cpu)
+{
+	unsigned long rate;
+
+	rate = clk_get_rate(clk_cpu);
+	return rate / 1000;
+}
+
+static int tegra_set_policy(struct cpufreq_policy *pol)
+{
+	NvError e = NvRmDfsSetCpuEnvelope(rm_cpufreq, pol->min, pol->max);
+
+	if (e) {
+		pr_err("%s: error 0x%08x \n", __func__, e);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+int tegra_start_dvfsd(void) {
+	int rc = 0;
+	static bool started = false;
+
+	mutex_lock(&init_mutex);
+	if (cpufreq_dfsd && !started) {
+		wake_up_process(cpufreq_dfsd);
+		started = true;
+	} else
+		rc = -ENOSYS;
+	mutex_unlock(&init_mutex);
+
+	return rc;
+
+}
+
+static int tegra_cpufreq_init_once(void)
+{
+	struct sched_param sp;
+	int rc = 0;
+
+	mutex_lock(&init_mutex);
+
+	if (rm_cpufreq)
+		goto clean;
+
+	if (NvRmOpenNew(&rm_cpufreq)!=NvSuccess) {
+		pr_err("%s: unable to open NvRm\n", __func__);
+		rc = -ENOSYS;
+		goto clean;
+	}
+
+	clk_cpu = clk_get_sys(NULL, "cpu");
+	if (IS_ERR(clk_cpu)) {
+		rc = PTR_ERR(clk_cpu);
+		clk_cpu = NULL;
+		goto clean;
+	}
+
+	cpufreq_dfsd = kthread_create(tegra_cpufreq_dfsd, NULL, "cpufreq-dvfsd");
+	if (IS_ERR(cpufreq_dfsd)) {
+		pr_err("%s: unable to start DVFS daemon\n", __func__);
+		rc = PTR_ERR(cpufreq_dfsd);
+		cpufreq_dfsd = NULL;
+		goto clean;
+	}
+
+	sp.sched_priority = DEFAULT_PRIO - 1;
+	if (sched_setscheduler_nocheck(cpufreq_dfsd, SCHED_FIFO, &sp) < 0)
+		pr_err("%s: unable to elevate DVFS daemon priority\n",__func__);
+
+clean:
+	if (rc) {
+		if (rm_cpufreq)
+			NvRmClose(rm_cpufreq);
+		if (clk_cpu)
+			clk_put(clk_cpu);
+		clk_cpu = NULL;
+		rm_cpufreq = NULL;
+	}
+
+	mutex_unlock(&init_mutex);
+	return rc;
+}
+
+static int tegra_cpufreq_driver_init(struct cpufreq_policy *pol)
+{
+	NvRmDfsClockUsage usage;
+	NvError e;
+	int rc;
+
+	rc = tegra_cpufreq_init_once();
+	if (rc)
+		return rc;
+
+	e = NvRmDfsGetClockUtilization(rm_cpufreq, NvRmDfsClockId_Cpu, &usage);
+
+	if (e != NvSuccess) {
+		WARN_ON(1);
+		return -ENXIO;
+	}
+
+	pr_debug("%s: min: %u max: %u current: %u\n",
+		 __func__, usage.MinKHz, usage.MaxKHz, usage.CurrentKHz);
+
+	pol->min = usage.LowCornerKHz;
+	pol->max = usage.HighCornerKHz;
+	pol->cur = usage.CurrentKHz;
+
+	pol->cpuinfo.min_freq = usage.MinKHz;
+	pol->cpuinfo.max_freq = usage.MaxKHz;
+	pol->cpuinfo.transition_latency = 0;
+
+	return 0;
+}
+
+static struct cpufreq_driver s_tegra_cpufreq_driver = {
+	.flags		= CPUFREQ_CONST_LOOPS,
+	.verify		= tegra_verify_speed,
+	.setpolicy	= tegra_set_policy,
+	.get		= tegra_get_speed,
+	.init		= tegra_cpufreq_driver_init,
+	.name		= "tegra_cpufreq",
+	.owner		= THIS_MODULE,
+
+};
+
+static int __init tegra_cpufreq_init(void)
+{
+	return cpufreq_register_driver(&s_tegra_cpufreq_driver);
+}
+
+static void __exit tegra_cpufreq_exit(void)
+{
+	kthread_stop(cpufreq_dfsd);
+	clk_put(clk_cpu);
+
+	cpufreq_unregister_driver(&s_tegra_cpufreq_driver);
+}
+
+MODULE_DESCRIPTION("CPU frequency driver for the Tegra SOC");
+MODULE_LICENSE("GPL");
+module_init(tegra_cpufreq_init);
+module_exit(tegra_cpufreq_exit);