ARM: tegra: Add dvfs

Change-Id: I865e52cae592507c642b92dde3a8293db2d0228f
Signed-off-by: Colin Cross <ccross@android.com>

9 files changed, 1208 insertions, 52 deletions

diff --git a/arch/arm/mach-tegra/Kconfig b/arch/arm/mach-tegra/Kconfig
index 82d05a9e2563..47771f214bc4 100644
--- a/arch/arm/mach-tegra/Kconfig
+++ b/arch/arm/mach-tegra/Kconfig
@@ -120,6 +120,15 @@ config TEGRA_EMC_SCALING_ENABLE
 
 endif
 
+config TEGRA_CPU_DVFS
+	bool "Enable voltage scaling on Tegra CPU"
+	default y
+
+config TEGRA_CORE_DVFS
+	bool "Enable voltage scaling on Tegra core"
+	depends on TEGRA_CPU_DVFS
+	default y
+
 config TEGRA_IOVMM_GART
 	bool "Enable I/O virtual memory manager for GART"
 	depends on ARCH_TEGRA_2x_SOC
diff --git a/arch/arm/mach-tegra/Makefile b/arch/arm/mach-tegra/Makefile
index 156ddaefc95c..89e3dcb98766 100644
--- a/arch/arm/mach-tegra/Makefile
+++ b/arch/arm/mach-tegra/Makefile
@@ -23,7 +23,9 @@ obj-$(CONFIG_TEGRA_PWM)                 += pwm.o
 obj-$(CONFIG_TEGRA_ARB_SEMAPHORE)	+= arb_sema.o
 
 obj-$(CONFIG_ARCH_TEGRA_2x_SOC)         += clock.o
+obj-$(CONFIG_ARCH_TEGRA_2x_SOC)         += dvfs.o
 obj-$(CONFIG_ARCH_TEGRA_2x_SOC)         += tegra2_clocks.o
+obj-$(CONFIG_ARCH_TEGRA_2x_SOC)         += tegra2_dvfs.o
 obj-$(CONFIG_ARCH_TEGRA_2x_SOC)         += tegra2_fuse.o
 obj-$(CONFIG_ARCH_TEGRA_2x_SOC)		+= tegra2_emc.o
 obj-$(CONFIG_ARCH_TEGRA_2x_SOC)		+= wakeups-t2.o
diff --git a/arch/arm/mach-tegra/clock.c b/arch/arm/mach-tegra/clock.c
index 48d50d3cbde7..1f68b97a584c 100644
--- a/arch/arm/mach-tegra/clock.c
+++ b/arch/arm/mach-tegra/clock.c
@@ -24,7 +24,6 @@
 #include <linux/init.h>
 #include <linux/list.h>
 #include <linux/module.h>
-#include <linux/sched.h>
 #include <linux/seq_file.h>
 #include <linux/slab.h>
 
@@ -32,27 +31,46 @@
 
 #include "board.h"
 #include "clock.h"
+#include "dvfs.h"
 
 /*
  * Locking:
  *
- * Each struct clk has a spinlock.
+ * Each struct clk has a lock.  Depending on the cansleep flag, that lock
+ * may be a spinlock or a mutex.  For most clocks, the spinlock is sufficient,
+ * and using the spinlock allows the clock to be manipulated from an interrupt
+ * or while holding a spinlock.  Some clocks may need to adjust a regulator
+ * in order to maintain the required voltage for a new frequency.  Those
+ * clocks set the cansleep flag, and take a mutex so that the regulator api
+ * can be used while holding the lock.
  *
  * To avoid AB-BA locking problems, locks must always be traversed from child
  * clock to parent clock.  For example, when enabling a clock, the clock's lock
  * is taken, and then clk_enable is called on the parent, which take's the
- * parent clock's lock.  There is one exceptions to this ordering: When dumping
- * the clock tree through debugfs.  In this case, clk_lock_all is called,
- * which attemps to iterate through the entire list of clocks and take every
- * clock lock.  If any call to spin_trylock fails, all locked clocks are
- * unlocked, and the process is retried.  When all the locks are held,
- * the only clock operation that can be called is clk_get_rate_all_locked.
+ * parent clock's lock.  There are two exceptions to this ordering:
+ *  1. When setting a clock as cansleep, in which case the entire list of clocks
+ *     is traversed to set the children as cansleep as well.  This must occur
+ *     during init, before any calls to clk_get, so no other clock locks can
+ *     get taken.
+ *  2. When dumping the clock tree through debugfs.  In this case, clk_lock_all
+ *     is called, which attemps to iterate through the entire list of clocks
+ *     and take every clock lock.  If any call to clk_trylock fails, a locked
+ *     clocks are unlocked, and the process is retried.  When all the locks
+ *     are held, the only clock operation that can be called is
+ *     clk_get_rate_all_locked.
  *
  * Within a single clock, no clock operation can call another clock operation
  * on itself, except for clk_get_rate_locked and clk_set_rate_locked.  Any
  * clock operation can call any other clock operation on any of it's possible
  * parents.
  *
+ * clk_set_cansleep is used to mark a clock as sleeping.  It is called during
+ * dvfs (Dynamic Voltage and Frequency Scaling) init on any clock that has a
+ * dvfs requirement.  It can only be called on clocks that are the sole parent
+ * of all of their child clocks, meaning the child clock can not be reparented
+ * onto a different, possibly non-sleeping, clock.  This is inherently true
+ * of all leaf clocks in the clock tree
+ *
  * An additional mutex, clock_list_lock, is used to protect the list of all
  * clocks.
  *
@@ -77,7 +95,7 @@ struct clk *tegra_get_clock_by_name(const char *name)
 	return ret;
 }
 
-/* Must be called with c->spinlock held */
+/* Must be called with clk_lock(c) held */
 static unsigned long clk_predict_rate_from_parent(struct clk *c, struct clk *p)
 {
 	u64 rate;
@@ -93,7 +111,7 @@ static unsigned long clk_predict_rate_from_parent(struct clk *c, struct clk *p)
 	return rate;
 }
 
-/* Must be called with c->spinlock held */
+/* Must be called with clk_lock(c) held */
 unsigned long clk_get_rate_locked(struct clk *c)
 {
 	unsigned long rate;
@@ -111,16 +129,46 @@ unsigned long clk_get_rate(struct clk *c)
 	unsigned long flags;
 	unsigned long rate;
 
-	spin_lock_irqsave(&c->spinlock, flags);
+	clk_lock_save(c, &flags);
 
 	rate = clk_get_rate_locked(c);
 
-	spin_unlock_irqrestore(&c->spinlock, flags);
+	clk_unlock_restore(c, &flags);
 
 	return rate;
 }
 EXPORT_SYMBOL(clk_get_rate);
 
+static void __clk_set_cansleep(struct clk *c)
+{
+	struct clk *child;
+	BUG_ON(mutex_is_locked(&c->mutex));
+	BUG_ON(spin_is_locked(&c->spinlock));
+
+	list_for_each_entry(child, &clocks, node) {
+		if (child->parent != c)
+			continue;
+
+		WARN(child->ops && child->ops->set_parent,
+			"can't make child clock %s of %s "
+			"sleepable if it's parent could change",
+			child->name, c->name);
+
+		__clk_set_cansleep(child);
+	}
+
+	c->cansleep = true;
+}
+
+/* Must be called before any clk_get calls */
+void clk_set_cansleep(struct clk *c)
+{
+
+	mutex_lock(&clock_list_lock);
+	__clk_set_cansleep(c);
+	mutex_unlock(&clock_list_lock);
+}
+
 int clk_reparent(struct clk *c, struct clk *parent)
 {
 	c->parent = parent;
@@ -129,7 +177,7 @@ int clk_reparent(struct clk *c, struct clk *parent)
 
 void clk_init(struct clk *c)
 {
-	spin_lock_init(&c->spinlock);
+	clk_lock_init(c);
 
 	if (c->ops && c->ops->init)
 		c->ops->init(c);
@@ -153,7 +201,13 @@ int clk_enable(struct clk *c)
 	int ret = 0;
 	unsigned long flags;
 
-	spin_lock_irqsave(&c->spinlock, flags);
+	clk_lock_save(c, &flags);
+
+	if (clk_is_auto_dvfs(c)) {
+		ret = tegra_dvfs_set_rate(c, clk_get_rate_locked(c));
+		if (ret)
+			goto out;
+	}
 
 	if (c->refcnt == 0) {
 		if (c->parent) {
@@ -175,7 +229,7 @@ int clk_enable(struct clk *c)
 	}
 	c->refcnt++;
 out:
-	spin_unlock_irqrestore(&c->spinlock, flags);
+	clk_unlock_restore(c, &flags);
 	return ret;
 }
 EXPORT_SYMBOL(clk_enable);
@@ -184,11 +238,11 @@ void clk_disable(struct clk *c)
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&c->spinlock, flags);
+	clk_lock_save(c, &flags);
 
 	if (c->refcnt == 0) {
 		WARN(1, "Attempting to disable clock %s with refcnt 0", c->name);
-		spin_unlock_irqrestore(&c->spinlock, flags);
+		clk_unlock_restore(c, &flags);
 		return;
 	}
 	if (c->refcnt == 1) {
@@ -202,18 +256,21 @@ void clk_disable(struct clk *c)
 	}
 	c->refcnt--;
 
-	spin_unlock_irqrestore(&c->spinlock, flags);
+	if (clk_is_auto_dvfs(c) && c->refcnt == 0)
+		tegra_dvfs_set_rate(c, 0);
+
+	clk_unlock_restore(c, &flags);
 }
 EXPORT_SYMBOL(clk_disable);
 
 int clk_set_parent(struct clk *c, struct clk *parent)
 {
-	int ret;
+	int ret = 0;
 	unsigned long flags;
 	unsigned long new_rate;
 	unsigned long old_rate;
 
-	spin_lock_irqsave(&c->spinlock, flags);
+	clk_lock_save(c, &flags);
 
 	if (!c->ops || !c->ops->set_parent) {
 		ret = -ENOSYS;
@@ -223,12 +280,23 @@ int clk_set_parent(struct clk *c, struct clk *parent)
 	new_rate = clk_predict_rate_from_parent(c, parent);
 	old_rate = clk_get_rate_locked(c);
 
+	if (clk_is_auto_dvfs(c) && c->refcnt > 0 &&
+			(!c->parent || new_rate > old_rate)) {
+		ret = tegra_dvfs_set_rate(c, new_rate);
+		if (ret)
+			goto out;
+	}
+
 	ret = c->ops->set_parent(c, parent);
 	if (ret)
 		goto out;
 
+	if (clk_is_auto_dvfs(c) && c->refcnt > 0 &&
+			new_rate < old_rate)
+		ret = tegra_dvfs_set_rate(c, new_rate);
+
 out:
-	spin_unlock_irqrestore(&c->spinlock, flags);
+	clk_unlock_restore(c, &flags);
 	return ret;
 }
 EXPORT_SYMBOL(clk_set_parent);
@@ -241,10 +309,11 @@ EXPORT_SYMBOL(clk_get_parent);
 
 int clk_set_rate_locked(struct clk *c, unsigned long rate)
 {
+	int ret = 0;
+	unsigned long old_rate;
 	long new_rate;
 
-	if (!c->ops || !c->ops->set_rate)
-		return -ENOSYS;
+	old_rate = clk_get_rate_locked(c);
 
 	if (rate > c->max_rate)
 		rate = c->max_rate;
@@ -252,31 +321,48 @@ int clk_set_rate_locked(struct clk *c, unsigned long rate)
 	if (c->ops && c->ops->round_rate) {
 		new_rate = c->ops->round_rate(c, rate);
 
-		if (new_rate < 0)
-			return new_rate;
+		if (new_rate < 0) {
+			ret = new_rate;
+			return ret;
+		}
 
 		rate = new_rate;
 	}
 
-	return c->ops->set_rate(c, rate);
+	if (clk_is_auto_dvfs(c) && rate > old_rate && c->refcnt > 0) {
+		ret = tegra_dvfs_set_rate(c, rate);
+		if (ret)
+			return ret;
+	}
+
+	ret = c->ops->set_rate(c, rate);
+	if (ret)
+		return ret;
+
+	if (clk_is_auto_dvfs(c) && rate < old_rate && c->refcnt > 0)
+		ret = tegra_dvfs_set_rate(c, rate);
+
+	return ret;
 }
 
 int clk_set_rate(struct clk *c, unsigned long rate)
 {
-	int ret;
 	unsigned long flags;
+	int ret;
 
-	spin_lock_irqsave(&c->spinlock, flags);
+	if (!c->ops || !c->ops->set_rate)
+		return -ENOSYS;
+
+	clk_lock_save(c, &flags);
 
 	ret = clk_set_rate_locked(c, rate);
 
-	spin_unlock_irqrestore(&c->spinlock, flags);
+	clk_unlock_restore(c, &flags);
 
 	return ret;
 }
 EXPORT_SYMBOL(clk_set_rate);
 
-
 /* Must be called with clocks lock and all indvidual clock locks held */
 unsigned long clk_get_rate_all_locked(struct clk *c)
 {
@@ -306,7 +392,7 @@ long clk_round_rate(struct clk *c, unsigned long rate)
 	unsigned long flags;
 	long ret;
 
-	spin_lock_irqsave(&c->spinlock, flags);
+	clk_lock_save(c, &flags);
 
 	if (!c->ops || !c->ops->round_rate) {
 		ret = -ENOSYS;
@@ -319,7 +405,7 @@ long clk_round_rate(struct clk *c, unsigned long rate)
 	ret = c->ops->round_rate(c, rate);
 
 out:
-	spin_unlock_irqrestore(&c->spinlock, flags);
+	clk_unlock_restore(c, &flags);
 	return ret;
 }
 EXPORT_SYMBOL(clk_round_rate);
@@ -400,6 +486,7 @@ EXPORT_SYMBOL(tegra_periph_reset_assert);
 void __init tegra_init_clock(void)
 {
 	tegra2_init_clocks();
+	tegra2_init_dvfs();
 }
 
 /*
@@ -411,9 +498,9 @@ void tegra_sdmmc_tap_delay(struct clk *c, int delay)
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&c->spinlock, flags);
+	clk_lock_save(c, &flags);
 	tegra2_sdmmc_tap_delay(c, delay);
-	spin_unlock_irqrestore(&c->spinlock, flags);
+	clk_unlock_restore(c, &flags);
 }
 
 static bool tegra_keep_boot_clocks = false;
@@ -430,13 +517,13 @@ __setup("tegra_keep_boot_clocks", tegra_keep_boot_clocks_setup);
  */
 static int __init tegra_init_disable_boot_clocks(void)
 {
+	unsigned long flags;
 	struct clk *c;
 
 	mutex_lock(&clock_list_lock);
 
 	list_for_each_entry(c, &clocks, node) {
-		spin_lock_irq(&c->spinlock);
-
+		clk_lock_save(c, &flags);
 		if (c->refcnt == 0 && c->state == ON &&
 				c->ops && c->ops->disable) {
 			pr_warn_once("%s clocks left on by bootloader:\n",
@@ -451,41 +538,80 @@ static int __init tegra_init_disable_boot_clocks(void)
 				c->state = OFF;
 			}
 		}
-
-		spin_unlock_irq(&c->spinlock);
+		clk_unlock_restore(c, &flags);
 	}
 
 	mutex_unlock(&clock_list_lock);
-
 	return 0;
 }
 late_initcall(tegra_init_disable_boot_clocks);
 
 #ifdef CONFIG_DEBUG_FS
 
+/*
+ * Attempt to lock all the clocks that are marked cansleep
+ * Must be called with irqs enabled
+ */
+static int __clk_lock_all_mutexes(void)
+{
+	struct clk *c;
+
+	might_sleep();
+
+	list_for_each_entry(c, &clocks, node)
+		if (clk_cansleep(c))
+			if (!mutex_trylock(&c->mutex))
+				goto unlock_mutexes;
+
+	return 0;
+
+unlock_mutexes:
+	list_for_each_entry_continue_reverse(c, &clocks, node)
+		if (clk_cansleep(c))
+			mutex_unlock(&c->mutex);
+
+	return -EAGAIN;
+}
+
+/*
+ * Attempt to lock all the clocks that are not marked cansleep
+ * Must be called with irqs disabled
+ */
 static int __clk_lock_all_spinlocks(void)
 {
 	struct clk *c;
 
 	list_for_each_entry(c, &clocks, node)
-		if (!spin_trylock(&c->spinlock))
-			goto unlock_spinlocks;
+		if (!clk_cansleep(c))
+			if (!spin_trylock(&c->spinlock))
+				goto unlock_spinlocks;
 
 	return 0;
 
 unlock_spinlocks:
 	list_for_each_entry_continue_reverse(c, &clocks, node)
-		spin_unlock(&c->spinlock);
+		if (!clk_cansleep(c))
+			spin_unlock(&c->spinlock);
 
 	return -EAGAIN;
 }
 
+static void __clk_unlock_all_mutexes(void)
+{
+	struct clk *c;
+
+	list_for_each_entry_reverse(c, &clocks, node)
+		if (clk_cansleep(c))
+			mutex_unlock(&c->mutex);
+}
+
 static void __clk_unlock_all_spinlocks(void)
 {
 	struct clk *c;
 
 	list_for_each_entry_reverse(c, &clocks, node)
-		spin_unlock(&c->spinlock);
+		if (!clk_cansleep(c))
+			spin_unlock(&c->spinlock);
 }
 
 /*
@@ -498,6 +624,10 @@ static void clk_lock_all(void)
 {
 	int ret;
 retry:
+	ret = __clk_lock_all_mutexes();
+	if (ret)
+		goto failed_mutexes;
+
 	local_irq_disable();
 
 	ret = __clk_lock_all_spinlocks();
@@ -509,7 +639,9 @@ retry:
 
 failed_spinlocks:
 	local_irq_enable();
-	yield();
+	__clk_unlock_all_mutexes();
+failed_mutexes:
+	msleep(1);
 	goto retry;
 }
 
@@ -523,10 +655,20 @@ static void clk_unlock_all(void)
 	__clk_unlock_all_spinlocks();
 
 	local_irq_enable();
+
+	__clk_unlock_all_mutexes();
 }
 
 static struct dentry *clk_debugfs_root;
 
+static void dvfs_show_one(struct seq_file *s, struct dvfs *d, int level)
+{
+	seq_printf(s, "%*s  %-*s%21s%d mV\n",
+			level * 3 + 1, "",
+			30 - level * 3, d->dvfs_rail->reg_id,
+			"",
+			d->cur_millivolts);
+}
 
 static void clock_tree_show_one(struct seq_file *s, struct clk *c, int level)
 {
@@ -563,6 +705,9 @@ static void clock_tree_show_one(struct seq_file *s, struct clk *c, int level)
 		30 - level * 3, c->name,
 		state, c->refcnt, div, clk_get_rate_all_locked(c));
 
+	if (c->dvfs)
+		dvfs_show_one(s, c->dvfs, level + 1);
+
 	list_for_each_entry(child, &clocks, node) {
 		if (child->parent != c)
 			continue;
@@ -698,6 +843,9 @@ static int __init clk_debugfs_init(void)
 	if (!d)
 		goto err_out;
 
+	if (dvfs_debugfs_init(clk_debugfs_root))
+		goto err_out;
+
 	list_for_each_entry(c, &clocks, node) {
 		err = clk_debugfs_register(c);
 		if (err)
diff --git a/arch/arm/mach-tegra/clock.h b/arch/arm/mach-tegra/clock.h
index 688316abc64e..1e5559bdc2f8 100644
--- a/arch/arm/mach-tegra/clock.h
+++ b/arch/arm/mach-tegra/clock.h
@@ -22,6 +22,7 @@
 
 #include <linux/clkdev.h>
 #include <linux/list.h>
+#include <linux/mutex.h>
 #include <linux/spinlock.h>
 
 #define DIV_BUS			(1 << 0)
@@ -76,6 +77,7 @@ enum clk_state {
 struct clk {
 	/* node for master clocks list */
 	struct list_head	node;		/* node for list of all clocks */
+	struct dvfs 		*dvfs;
 	struct clk_lookup	lookup;
 
 #ifdef CONFIG_DEBUG_FS
@@ -83,9 +85,12 @@ struct clk {
 #endif
 	bool			set;
 	struct clk_ops		*ops;
+	unsigned long		dvfs_rate;
 	unsigned long		rate;
 	unsigned long		max_rate;
 	unsigned long		min_rate;
+	bool			auto_dvfs;
+	bool			cansleep;
 	u32			flags;
 	const char		*name;
 
@@ -130,6 +135,7 @@ struct clk {
 		} shared_bus_user;
 	} u;
 
+	struct mutex mutex;
 	spinlock_t spinlock;
 };
 
@@ -153,8 +159,48 @@ struct clk *tegra_get_clock_by_name(const char *name);
 unsigned long clk_measure_input_freq(void);
 int clk_reparent(struct clk *c, struct clk *parent);
 void tegra_clk_init_from_table(struct tegra_clk_init_table *table);
+void clk_set_cansleep(struct clk *c);
 unsigned long clk_get_rate_locked(struct clk *c);
 int clk_set_rate_locked(struct clk *c, unsigned long rate);
 void tegra2_sdmmc_tap_delay(struct clk *c, int delay);
 
+static inline bool clk_is_auto_dvfs(struct clk *c)
+{
+	return c->auto_dvfs;
+}
+
+static inline bool clk_is_dvfs(struct clk *c)
+{
+	return (c->dvfs != NULL);
+}
+
+static inline bool clk_cansleep(struct clk *c)
+{
+	return c->cansleep;
+}
+
+static inline void clk_lock_save(struct clk *c, unsigned long *flags)
+{
+	if (clk_cansleep(c)) {
+		*flags = 0;
+		mutex_lock(&c->mutex);
+	} else {
+		spin_lock_irqsave(&c->spinlock, *flags);
+	}
+}
+
+static inline void clk_unlock_restore(struct clk *c, unsigned long *flags)
+{
+	if (clk_cansleep(c))
+		mutex_unlock(&c->mutex);
+	else
+		spin_unlock_irqrestore(&c->spinlock, *flags);
+}
+
+static inline void clk_lock_init(struct clk *c)
+{
+	mutex_init(&c->mutex);
+	spin_lock_init(&c->spinlock);
+}
+
 #endif
diff --git a/arch/arm/mach-tegra/dvfs.c b/arch/arm/mach-tegra/dvfs.c
new file mode 100644
index 000000000000..55891c13d497
--- /dev/null
+++ b/arch/arm/mach-tegra/dvfs.c
@@ -0,0 +1,557 @@
+/*
+ *
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * Author:
+ *	Colin Cross <ccross@google.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/debugfs.h>
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/list_sort.h>
+#include <linux/module.h>
+#include <linux/regulator/consumer.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+#include <linux/suspend.h>
+#include <linux/delay.h>
+
+#include <mach/clk.h>
+
+#include "board.h"
+#include "clock.h"
+#include "dvfs.h"
+
+static LIST_HEAD(dvfs_rail_list);
+static DEFINE_MUTEX(dvfs_lock);
+
+static int dvfs_rail_update(struct dvfs_rail *rail);
+
+void tegra_dvfs_add_relationships(struct dvfs_relationship *rels, int n)
+{
+	int i;
+	struct dvfs_relationship *rel;
+
+	mutex_lock(&dvfs_lock);
+
+	for (i = 0; i < n; i++) {
+		rel = &rels[i];
+		list_add_tail(&rel->from_node, &rel->to->relationships_from);
+		list_add_tail(&rel->to_node, &rel->from->relationships_to);
+	}
+
+	mutex_unlock(&dvfs_lock);
+}
+
+int tegra_dvfs_init_rails(struct dvfs_rail *rails[], int n)
+{
+	int i;
+
+	mutex_lock(&dvfs_lock);
+
+	for (i = 0; i < n; i++) {
+		INIT_LIST_HEAD(&rails[i]->dvfs);
+		INIT_LIST_HEAD(&rails[i]->relationships_from);
+		INIT_LIST_HEAD(&rails[i]->relationships_to);
+		rails[i]->millivolts = rails[i]->nominal_millivolts;
+		rails[i]->new_millivolts = rails[i]->nominal_millivolts;
+		if (!rails[i]->step)
+			rails[i]->step = rails[i]->max_millivolts;
+
+		list_add_tail(&rails[i]->node, &dvfs_rail_list);
+	}
+
+	mutex_unlock(&dvfs_lock);
+
+	return 0;
+};
+
+static int dvfs_solve_relationship(struct dvfs_relationship *rel)
+{
+	return rel->solve(rel->from, rel->to);
+}
+
+/* Sets the voltage on a dvfs rail to a specific value, and updates any
+ * rails that depend on this rail. */
+static int dvfs_rail_set_voltage(struct dvfs_rail *rail, int millivolts)
+{
+	int ret = 0;
+	struct dvfs_relationship *rel;
+	int step = (millivolts > rail->millivolts) ? rail->step : -rail->step;
+	int i;
+	int steps;
+
+	if (!rail->reg) {
+		if (millivolts == rail->millivolts)
+			return 0;
+		else
+			return -EINVAL;
+	}
+
+	if (rail->disabled)
+		return 0;
+
+	steps = DIV_ROUND_UP(abs(millivolts - rail->millivolts), rail->step);
+
+	for (i = 0; i < steps; i++) {
+		if (abs(millivolts - rail->millivolts) > rail->step)
+			rail->new_millivolts = rail->millivolts + step;
+		else
+			rail->new_millivolts = millivolts;
+
+		/* Before changing the voltage, tell each rail that depends
+		 * on this rail that the voltage will change.
+		 * This rail will be the "from" rail in the relationship,
+		 * the rail that depends on this rail will be the "to" rail.
+		 * from->millivolts will be the old voltage
+		 * from->new_millivolts will be the new voltage */
+		list_for_each_entry(rel, &rail->relationships_to, to_node) {
+			ret = dvfs_rail_update(rel->to);
+			if (ret)
+				return ret;
+		}
+
+		if (!rail->disabled) {
+			ret = regulator_set_voltage(rail->reg,
+				rail->new_millivolts * 1000,
+				rail->max_millivolts * 1000);
+		}
+		if (ret) {
+			pr_err("Failed to set dvfs regulator %s\n", rail->reg_id);
+			return ret;
+		}
+
+		rail->millivolts = rail->new_millivolts;
+
+		/* After changing the voltage, tell each rail that depends
+		 * on this rail that the voltage has changed.
+		 * from->millivolts and from->new_millivolts will be the
+		 * new voltage */
+		list_for_each_entry(rel, &rail->relationships_to, to_node) {
+			ret = dvfs_rail_update(rel->to);
+			if (ret)
+				return ret;
+		}
+	}
+
+	if (unlikely(rail->millivolts != millivolts)) {
+		pr_err("%s: rail didn't reach target %d in %d steps (%d)\n",
+			__func__, millivolts, steps, rail->millivolts);
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+/* Determine the minimum valid voltage for a rail, taking into account
+ * the dvfs clocks and any rails that this rail depends on.  Calls
+ * dvfs_rail_set_voltage with the new voltage, which will call
+ * dvfs_rail_update on any rails that depend on this rail. */
+static int dvfs_rail_update(struct dvfs_rail *rail)
+{
+	int millivolts = 0;
+	struct dvfs *d;
+	struct dvfs_relationship *rel;
+	int ret = 0;
+
+	/* if dvfs is suspended, return and handle it during resume */
+	if (rail->suspended)
+		return 0;
+
+	/* if regulators are not connected yet, return and handle it later */
+	if (!rail->reg)
+		return 0;
+
+	/* Find the maximum voltage requested by any clock */
+	list_for_each_entry(d, &rail->dvfs, reg_node)
+		millivolts = max(d->cur_millivolts, millivolts);
+
+	rail->new_millivolts = millivolts;
+
+	/* Check any rails that this rail depends on */
+	list_for_each_entry(rel, &rail->relationships_from, from_node)
+		rail->new_millivolts = dvfs_solve_relationship(rel);
+
+	if (rail->new_millivolts != rail->millivolts)
+		ret = dvfs_rail_set_voltage(rail, rail->new_millivolts);
+
+	return ret;
+}
+
+static int dvfs_rail_connect_to_regulator(struct dvfs_rail *rail)
+{
+	struct regulator *reg;
+
+	if (!rail->reg) {
+		reg = regulator_get(NULL, rail->reg_id);
+		if (IS_ERR(reg))
+			return -EINVAL;
+	}
+
+	rail->reg = reg;
+
+	return 0;
+}
+
+static int
+__tegra_dvfs_set_rate(struct dvfs *d, unsigned long rate)
+{
+	int i = 0;
+	int ret;
+
+	if (d->freqs == NULL || d->millivolts == NULL)
+		return -ENODEV;
+
+	if (rate > d->freqs[d->num_freqs - 1]) {
+		pr_warn("tegra_dvfs: rate %lu too high for dvfs on %s\n", rate,
+			d->clk_name);
+		return -EINVAL;
+	}
+
+	if (rate == 0) {
+		d->cur_millivolts = 0;
+	} else {
+		while (i < d->num_freqs && rate > d->freqs[i])
+			i++;
+
+		d->cur_millivolts = d->millivolts[i];
+	}
+
+	d->cur_rate = rate;
+
+	ret = dvfs_rail_update(d->dvfs_rail);
+	if (ret)
+		pr_err("Failed to set regulator %s for clock %s to %d mV\n",
+			d->dvfs_rail->reg_id, d->clk_name, d->cur_millivolts);
+
+	return ret;
+}
+
+int tegra_dvfs_set_rate(struct clk *c, unsigned long rate)
+{
+	int ret;
+
+	if (!c->dvfs)
+		return -EINVAL;
+
+	mutex_lock(&dvfs_lock);
+	ret = __tegra_dvfs_set_rate(c->dvfs, rate);
+	mutex_unlock(&dvfs_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(tegra_dvfs_set_rate);
+
+/* May only be called during clock init, does not take any locks on clock c. */
+int __init tegra_enable_dvfs_on_clk(struct clk *c, struct dvfs *d)
+{
+	int i;
+
+	if (c->dvfs) {
+		pr_err("Error when enabling dvfs on %s for clock %s:\n",
+			d->dvfs_rail->reg_id, c->name);
+		pr_err("DVFS already enabled for %s\n",
+			c->dvfs->dvfs_rail->reg_id);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < MAX_DVFS_FREQS; i++) {
+		if (d->millivolts[i] == 0)
+			break;
+
+		d->freqs[i] *= d->freqs_mult;
+
+		/* If final frequencies are 0, pad with previous frequency */
+		if (d->freqs[i] == 0 && i > 1)
+			d->freqs[i] = d->freqs[i - 1];
+	}
+	d->num_freqs = i;
+
+	if (d->auto_dvfs) {
+		c->auto_dvfs = true;
+		clk_set_cansleep(c);
+	}
+
+	c->dvfs = d;
+
+	mutex_lock(&dvfs_lock);
+	list_add_tail(&d->reg_node, &d->dvfs_rail->dvfs);
+	mutex_unlock(&dvfs_lock);
+
+	return 0;
+}
+
+static bool tegra_dvfs_all_rails_suspended(void)
+{
+	struct dvfs_rail *rail;
+	bool all_suspended = true;
+
+	list_for_each_entry(rail, &dvfs_rail_list, node)
+		if (!rail->suspended && !rail->disabled)
+			all_suspended = false;
+
+	return all_suspended;
+}
+
+static bool tegra_dvfs_from_rails_suspended(struct dvfs_rail *to)
+{
+	struct dvfs_relationship *rel;
+	bool all_suspended = true;
+
+	list_for_each_entry(rel, &to->relationships_from, from_node)
+		if (!rel->from->suspended && !rel->from->disabled)
+			all_suspended = false;
+
+	return all_suspended;
+}
+
+static int tegra_dvfs_suspend_one(void)
+{
+	struct dvfs_rail *rail;
+	int ret;
+
+	list_for_each_entry(rail, &dvfs_rail_list, node) {
+		if (!rail->suspended && !rail->disabled &&
+		    tegra_dvfs_from_rails_suspended(rail)) {
+			ret = dvfs_rail_set_voltage(rail,
+				rail->nominal_millivolts);
+			if (ret)
+				return ret;
+			rail->suspended = true;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static void tegra_dvfs_resume(void)
+{
+	struct dvfs_rail *rail;
+
+	mutex_lock(&dvfs_lock);
+
+	list_for_each_entry(rail, &dvfs_rail_list, node)
+		rail->suspended = false;
+
+	list_for_each_entry(rail, &dvfs_rail_list, node)
+		dvfs_rail_update(rail);
+
+	mutex_unlock(&dvfs_lock);
+}
+
+static int tegra_dvfs_suspend(void)
+{
+	int ret = 0;
+
+	mutex_lock(&dvfs_lock);
+
+	while (!tegra_dvfs_all_rails_suspended()) {
+		ret = tegra_dvfs_suspend_one();
+		if (ret)
+			break;
+	}
+
+	mutex_unlock(&dvfs_lock);
+
+	if (ret)
+		tegra_dvfs_resume();
+
+	return ret;
+}
+
+static int tegra_dvfs_pm_notify(struct notifier_block *nb,
+				unsigned long event, void *data)
+{
+	switch (event) {
+	case PM_SUSPEND_PREPARE:
+		if (tegra_dvfs_suspend())
+			return NOTIFY_STOP;
+		break;
+	case PM_POST_SUSPEND:
+		tegra_dvfs_resume();
+		break;
+	}
+
+	return NOTIFY_OK;
+};
+
+static struct notifier_block tegra_dvfs_nb = {
+	.notifier_call = tegra_dvfs_pm_notify,
+};
+
+/* must be called with dvfs lock held */
+static void __tegra_dvfs_rail_disable(struct dvfs_rail *rail)
+{
+	int ret;
+
+	if (!rail->disabled) {
+		ret = dvfs_rail_set_voltage(rail, rail->nominal_millivolts);
+		if (ret)
+			pr_info("dvfs: failed to set regulator %s to disable "
+				"voltage %d\n", rail->reg_id,
+				rail->nominal_millivolts);
+		rail->disabled = true;
+	}
+}
+
+/* must be called with dvfs lock held */
+static void __tegra_dvfs_rail_enable(struct dvfs_rail *rail)
+{
+	if (rail->disabled) {
+		rail->disabled = false;
+		dvfs_rail_update(rail);
+	}
+}
+
+void tegra_dvfs_rail_enable(struct dvfs_rail *rail)
+{
+	mutex_lock(&dvfs_lock);
+	__tegra_dvfs_rail_enable(rail);
+	mutex_unlock(&dvfs_lock);
+}
+
+void tegra_dvfs_rail_disable(struct dvfs_rail *rail)
+{
+	mutex_lock(&dvfs_lock);
+	__tegra_dvfs_rail_disable(rail);
+	mutex_unlock(&dvfs_lock);
+}
+
+int tegra_dvfs_rail_disable_by_name(const char *reg_id)
+{
+	struct dvfs_rail *rail;
+	int ret = 0;
+
+	mutex_lock(&dvfs_lock);
+	list_for_each_entry(rail, &dvfs_rail_list, node) {
+		if (!strcmp(reg_id, rail->reg_id)) {
+			__tegra_dvfs_rail_disable(rail);
+			goto out;
+		}
+	}
+
+	ret = -EINVAL;
+
+out:
+	mutex_unlock(&dvfs_lock);
+	return ret;
+}
+
+/*
+ * Iterate through all the dvfs regulators, finding the regulator exported
+ * by the regulator api for each one.  Must be called in late init, after
+ * all the regulator api's regulators are initialized.
+ */
+int __init tegra_dvfs_late_init(void)
+{
+	struct dvfs_rail *rail;
+
+	mutex_lock(&dvfs_lock);
+
+	list_for_each_entry(rail, &dvfs_rail_list, node)
+		dvfs_rail_connect_to_regulator(rail);
+
+	list_for_each_entry(rail, &dvfs_rail_list, node)
+		dvfs_rail_update(rail);
+
+	mutex_unlock(&dvfs_lock);
+
+	register_pm_notifier(&tegra_dvfs_nb);
+
+	return 0;
+}
+late_initcall(tegra_dvfs_late_init);
+
+#ifdef CONFIG_DEBUG_FS
+static int dvfs_tree_sort_cmp(void *p, struct list_head *a, struct list_head *b)
+{
+	struct dvfs *da = list_entry(a, struct dvfs, reg_node);
+	struct dvfs *db = list_entry(b, struct dvfs, reg_node);
+	int ret;
+
+	ret = strcmp(da->dvfs_rail->reg_id, db->dvfs_rail->reg_id);
+	if (ret != 0)
+		return ret;
+
+	if (da->cur_millivolts < db->cur_millivolts)
+		return 1;
+	if (da->cur_millivolts > db->cur_millivolts)
+		return -1;
+
+	return strcmp(da->clk_name, db->clk_name);
+}
+
+static int dvfs_tree_show(struct seq_file *s, void *data)
+{
+	struct dvfs *d;
+	struct dvfs_rail *rail;
+	struct dvfs_relationship *rel;
+
+	seq_printf(s, "   clock      rate       mV\n");
+	seq_printf(s, "--------------------------------\n");
+
+	mutex_lock(&dvfs_lock);
+
+	list_for_each_entry(rail, &dvfs_rail_list, node) {
+		seq_printf(s, "%s %d mV%s:\n", rail->reg_id,
+			rail->millivolts, rail->disabled ? " disabled" : "");
+		list_for_each_entry(rel, &rail->relationships_from, from_node) {
+			seq_printf(s, "   %-10s %-7d mV %-4d mV\n",
+				rel->from->reg_id,
+				rel->from->millivolts,
+				dvfs_solve_relationship(rel));
+		}
+
+		list_sort(NULL, &rail->dvfs, dvfs_tree_sort_cmp);
+
+		list_for_each_entry(d, &rail->dvfs, reg_node) {
+			seq_printf(s, "   %-10s %-10lu %-4d mV\n", d->clk_name,
+				d->cur_rate, d->cur_millivolts);
+		}
+	}
+
+	mutex_unlock(&dvfs_lock);
+
+	return 0;
+}
+
+static int dvfs_tree_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dvfs_tree_show, inode->i_private);
+}
+
+static const struct file_operations dvfs_tree_fops = {
+	.open		= dvfs_tree_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+int __init dvfs_debugfs_init(struct dentry *clk_debugfs_root)
+{
+	struct dentry *d;
+
+	d = debugfs_create_file("dvfs", S_IRUGO, clk_debugfs_root, NULL,
+		&dvfs_tree_fops);
+	if (!d)
+		return -ENOMEM;
+
+	return 0;
+}
+
+#endif
diff --git a/arch/arm/mach-tegra/dvfs.h b/arch/arm/mach-tegra/dvfs.h
new file mode 100644
index 000000000000..68622b899c59
--- /dev/null
+++ b/arch/arm/mach-tegra/dvfs.h
@@ -0,0 +1,93 @@
+/*
+ *
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * Author:
+ *	Colin Cross <ccross@google.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#ifndef _TEGRA_DVFS_H_
+#define _TEGRA_DVFS_H_
+
+#define MAX_DVFS_FREQS	16
+
+struct clk;
+struct dvfs_rail;
+
+/*
+ * dvfs_relationship between to rails, "from" and "to"
+ * when the rail changes, it will call dvfs_rail_update on the rails
+ * in the relationship_to list.
+ * when determining the voltage to set a rail to, it will consider each
+ * rail in the relationship_from list.
+ */
+struct dvfs_relationship {
+	struct dvfs_rail *to;
+	struct dvfs_rail *from;
+	int (*solve)(struct dvfs_rail *, struct dvfs_rail *);
+
+	struct list_head to_node; /* node in relationship_to list */
+	struct list_head from_node; /* node in relationship_from list */
+};
+
+struct dvfs_rail {
+	const char *reg_id;
+	int min_millivolts;
+	int max_millivolts;
+	int nominal_millivolts;
+	int step;
+	bool disabled;
+
+	struct list_head node;  /* node in dvfs_rail_list */
+	struct list_head dvfs;  /* list head of attached dvfs clocks */
+	struct list_head relationships_to;
+	struct list_head relationships_from;
+	struct regulator *reg;
+	int millivolts;
+	int new_millivolts;
+	bool suspended;
+};
+
+struct dvfs {
+	/* Used only by tegra2_clock.c */
+	const char *clk_name;
+	int cpu_process_id;
+
+	/* Must be initialized before tegra_dvfs_init */
+	int freqs_mult;
+	unsigned long freqs[MAX_DVFS_FREQS];
+	const int *millivolts;
+	struct dvfs_rail *dvfs_rail;
+	bool auto_dvfs;
+
+	/* Filled in by tegra_dvfs_init */
+	int max_millivolts;
+	int num_freqs;
+
+	int cur_millivolts;
+	unsigned long cur_rate;
+	struct list_head node;
+	struct list_head debug_node;
+	struct list_head reg_node;
+};
+
+void tegra2_init_dvfs(void);
+int tegra_enable_dvfs_on_clk(struct clk *c, struct dvfs *d);
+int dvfs_debugfs_init(struct dentry *clk_debugfs_root);
+int tegra_dvfs_late_init(void);
+int tegra_dvfs_init_rails(struct dvfs_rail *dvfs_rails[], int n);
+void tegra_dvfs_add_relationships(struct dvfs_relationship *rels, int n);
+void tegra_dvfs_rail_enable(struct dvfs_rail *rail);
+void tegra_dvfs_rail_disable(struct dvfs_rail *rail);
+
+#endif
diff --git a/arch/arm/mach-tegra/include/mach/clk.h b/arch/arm/mach-tegra/include/mach/clk.h
index c8baf8f80d23..b3f704ab913d 100644
--- a/arch/arm/mach-tegra/include/mach/clk.h
+++ b/arch/arm/mach-tegra/include/mach/clk.h
@@ -21,11 +21,14 @@
 #define __MACH_CLK_H
 
 struct clk;
+struct dvfs;
 
 void tegra_periph_reset_deassert(struct clk *c);
 void tegra_periph_reset_assert(struct clk *c);
 
+int tegra_dvfs_set_rate(struct clk *c, unsigned long rate);
 unsigned long clk_get_rate_all_locked(struct clk *c);
 void tegra_sdmmc_tap_delay(struct clk *c, int delay);
+int tegra_dvfs_rail_disable_by_name(const char *reg_id);
 
 #endif
diff --git a/arch/arm/mach-tegra/tegra2_clocks.c b/arch/arm/mach-tegra/tegra2_clocks.c
index a41f48744fb4..4f6610b858ce 100644
--- a/arch/arm/mach-tegra/tegra2_clocks.c
+++ b/arch/arm/mach-tegra/tegra2_clocks.c
@@ -1350,12 +1350,12 @@ static void tegra_clk_shared_bus_init(struct clk *c)
 	c->state = OFF;
 	c->set = true;
 
-	spin_lock_irqsave(&c->parent->spinlock, flags);
+	clk_lock_save(c->parent, &flags);
 
 	list_add_tail(&c->u.shared_bus_user.node,
 		&c->parent->shared_bus_list);
 
-	spin_unlock_irqrestore(&c->parent->spinlock, flags);
+	clk_unlock_restore(c->parent, &flags);
 }
 
 static int tegra_clk_shared_bus_set_rate(struct clk *c, unsigned long rate)
@@ -1368,12 +1368,12 @@ static int tegra_clk_shared_bus_set_rate(struct clk *c, unsigned long rate)
 	if (new_rate < 0)
 		return new_rate;
 
-	spin_lock_irqsave(&c->parent->spinlock, flags);
+	clk_lock_save(c->parent, &flags);
 
 	c->u.shared_bus_user.rate = new_rate;
 	ret = tegra_clk_shared_bus_update(c->parent);
 
-	spin_unlock_irqrestore(&c->parent->spinlock, flags);
+	clk_unlock_restore(c->parent, &flags);
 
 	return ret;
 }
@@ -1388,12 +1388,12 @@ static int tegra_clk_shared_bus_enable(struct clk *c)
 	unsigned long flags;
 	int ret;
 
-	spin_lock_irqsave(&c->parent->spinlock, flags);
+	clk_lock_save(c->parent, &flags);
 
 	c->u.shared_bus_user.enabled = true;
 	ret = tegra_clk_shared_bus_update(c->parent);
 
-	spin_unlock_irqrestore(&c->parent->spinlock, flags);
+	clk_unlock_restore(c->parent, &flags);
 
 	return ret;
 }
@@ -1403,13 +1403,13 @@ static void tegra_clk_shared_bus_disable(struct clk *c)
 	unsigned long flags;
 	int ret;
 
-	spin_lock_irqsave(&c->parent->spinlock, flags);
+	clk_lock_save(c->parent, &flags);
 
 	c->u.shared_bus_user.enabled = false;
 	ret = tegra_clk_shared_bus_update(c->parent);
 	WARN_ON_ONCE(ret);
 
-	spin_unlock_irqrestore(&c->parent->spinlock, flags);
+	clk_unlock_restore(c->parent, &flags);
 }
 
 static struct clk_ops tegra_clk_shared_bus_ops = {
diff --git a/arch/arm/mach-tegra/tegra2_dvfs.c b/arch/arm/mach-tegra/tegra2_dvfs.c
new file mode 100644
index 000000000000..9b5397414e03
--- /dev/null
+++ b/arch/arm/mach-tegra/tegra2_dvfs.c
@@ -0,0 +1,298 @@
+/*
+ * arch/arm/mach-tegra/tegra2_dvfs.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * Author:
+ *	Colin Cross <ccross@google.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/string.h>
+#include <linux/module.h>
+
+#include "clock.h"
+#include "dvfs.h"
+#include "fuse.h"
+
+#ifdef CONFIG_TEGRA_CORE_DVFS
+static bool tegra_dvfs_core_disabled;
+#else
+static bool tegra_dvfs_core_disabled = true;
+#endif
+#ifdef CONFIG_TEGRA_CPU_DVFS
+static bool tegra_dvfs_cpu_disabled;
+#else
+static bool tegra_dvfs_cpu_disabled = true;
+#endif
+
+static const int core_millivolts[MAX_DVFS_FREQS] =
+	{950, 1000, 1100, 1200, 1275};
+static const int cpu_millivolts[MAX_DVFS_FREQS] =
+	{750, 775, 800, 825, 875,  900,  925,  975,  1000, 1050, 1100};
+
+#define KHZ 1000
+#define MHZ 1000000
+
+static struct dvfs_rail tegra2_dvfs_rail_vdd_cpu = {
+	.reg_id = "vdd_cpu",
+	.max_millivolts = 1100,
+	.min_millivolts = 750,
+	.nominal_millivolts = 1100,
+};
+
+static struct dvfs_rail tegra2_dvfs_rail_vdd_core = {
+	.reg_id = "vdd_core",
+	.max_millivolts = 1275,
+	.min_millivolts = 950,
+	.nominal_millivolts = 1200,
+	.step = 150, /* step vdd_core by 150 mV to allow vdd_aon to follow */
+};
+
+static struct dvfs_rail tegra2_dvfs_rail_vdd_aon = {
+	.reg_id = "vdd_aon",
+	.max_millivolts = 1275,
+	.min_millivolts = 950,
+	.nominal_millivolts = 1200,
+#ifndef CONFIG_TEGRA_CORE_DVFS
+	.disabled = true,
+#endif
+};
+
+/* vdd_core and vdd_aon must be 50 mV higher than vdd_cpu */
+static int tegra2_dvfs_rel_vdd_cpu_vdd_core(struct dvfs_rail *vdd_cpu,
+	struct dvfs_rail *vdd_core)
+{
+	if (vdd_cpu->new_millivolts > vdd_cpu->millivolts &&
+	    vdd_core->new_millivolts < vdd_cpu->new_millivolts + 50)
+		return vdd_cpu->new_millivolts + 50;
+
+	if (vdd_core->new_millivolts < vdd_cpu->millivolts + 50)
+		return vdd_cpu->millivolts + 50;
+
+	return vdd_core->new_millivolts;
+}
+
+/* vdd_aon must be within 170 mV of vdd_core */
+static int tegra2_dvfs_rel_vdd_core_vdd_aon(struct dvfs_rail *vdd_core,
+	struct dvfs_rail *vdd_aon)
+{
+	BUG_ON(abs(vdd_aon->millivolts - vdd_core->millivolts) >
+		vdd_aon->step);
+	return vdd_core->millivolts;
+}
+
+static struct dvfs_relationship tegra2_dvfs_relationships[] = {
+	{
+		/* vdd_core must be 50 mV higher than vdd_cpu */
+		.from = &tegra2_dvfs_rail_vdd_cpu,
+		.to = &tegra2_dvfs_rail_vdd_core,
+		.solve = tegra2_dvfs_rel_vdd_cpu_vdd_core,
+	},
+	{
+		/* vdd_aon must be 50 mV higher than vdd_cpu */
+		.from = &tegra2_dvfs_rail_vdd_cpu,
+		.to = &tegra2_dvfs_rail_vdd_aon,
+		.solve = tegra2_dvfs_rel_vdd_cpu_vdd_core,
+	},
+	{
+		/* vdd_aon must be within 170 mV of vdd_core */
+		.from = &tegra2_dvfs_rail_vdd_core,
+		.to = &tegra2_dvfs_rail_vdd_aon,
+		.solve = tegra2_dvfs_rel_vdd_core_vdd_aon,
+	},
+};
+
+static struct dvfs_rail *tegra2_dvfs_rails[] = {
+	&tegra2_dvfs_rail_vdd_cpu,
+	&tegra2_dvfs_rail_vdd_core,
+	&tegra2_dvfs_rail_vdd_aon,
+};
+
+#define CPU_DVFS(_clk_name, _process_id, _mult, _freqs...)	\
+	{							\
+		.clk_name	= _clk_name,			\
+		.cpu_process_id	= _process_id,			\
+		.freqs		= {_freqs},			\
+		.freqs_mult	= _mult,			\
+		.millivolts	= cpu_millivolts,		\
+		.auto_dvfs	= true,				\
+		.dvfs_rail	= &tegra2_dvfs_rail_vdd_cpu,	\
+	}
+
+#define CORE_DVFS(_clk_name, _auto, _mult, _freqs...)		\
+	{							\
+		.clk_name	= _clk_name,			\
+		.cpu_process_id = -1,				\
+		.freqs		= {_freqs},			\
+		.freqs_mult	= _mult,			\
+		.millivolts	= core_millivolts,		\
+		.auto_dvfs	= _auto,			\
+		.dvfs_rail	= &tegra2_dvfs_rail_vdd_core,	\
+	}
+
+static struct dvfs dvfs_init[] = {
+	/* Cpu voltages (mV):   750, 775, 800, 825, 875, 900, 925, 975, 1000, 1050, 1100 */
+	CPU_DVFS("cpu", 0, MHZ, 314, 314, 314, 456, 456, 608, 608, 760, 817,  912,  1000),
+	CPU_DVFS("cpu", 1, MHZ, 314, 314, 314, 456, 456, 618, 618, 770, 827,  922,  1000),
+	CPU_DVFS("cpu", 2, MHZ, 494, 675, 675, 675, 817, 817, 922, 1000),
+	CPU_DVFS("cpu", 3, MHZ, 730, 760, 845, 845, 1000),
+
+	/* Core voltages (mV):       950,    1000,   1100,   1200,   1275 */
+	CORE_DVFS("emc",     1, KHZ, 57000,  333000, 333000, 666000, 666000),
+
+#if 0
+	/*
+	 * The sdhci core calls the clock ops with a spinlock held, which
+	 * conflicts with the sleeping dvfs api.
+	 * For now, boards must ensure that the core voltage does not drop
+	 * below 1V, or that the sdmmc busses are set to 44 MHz or less.
+	 */
+	CORE_DVFS("sdmmc1",  1, KHZ, 44000,  52000,  52000,  52000,  52000),
+	CORE_DVFS("sdmmc2",  1, KHZ, 44000,  52000,  52000,  52000,  52000),
+	CORE_DVFS("sdmmc3",  1, KHZ, 44000,  52000,  52000,  52000,  52000),
+	CORE_DVFS("sdmmc4",  1, KHZ, 44000,  52000,  52000,  52000,  52000),
+#endif
+
+	CORE_DVFS("ndflash", 1, KHZ, 130000, 150000, 158000, 164000, 164000),
+	CORE_DVFS("nor",     1, KHZ, 0,      92000,  92000,  92000,  92000),
+	CORE_DVFS("ide",     1, KHZ, 0,      0,      100000, 100000, 100000),
+	CORE_DVFS("mipi",    1, KHZ, 0,      40000,  40000,  40000, 60000),
+	CORE_DVFS("usbd",    1, KHZ, 0,      0,      0,      480000, 480000),
+	CORE_DVFS("usb2",    1, KHZ, 0,      0,      0,      480000, 480000),
+	CORE_DVFS("usb3",    1, KHZ, 0,      0,      0,      480000, 480000),
+	CORE_DVFS("pcie",    1, KHZ, 0,      0,      0,      250000, 250000),
+	CORE_DVFS("dsi",     1, KHZ, 100000, 100000, 100000, 500000, 500000),
+	CORE_DVFS("tvo",     1, KHZ, 0,      0,      0,      250000, 250000),
+
+	/*
+	 * The clock rate for the display controllers that determines the
+	 * necessary core voltage depends on a divider that is internal
+	 * to the display block.  Disable auto-dvfs on the display clocks,
+	 * and let the display driver call tegra_dvfs_set_rate manually
+	 */
+	CORE_DVFS("disp1",   0, KHZ, 158000, 158000, 190000, 190000, 190000),
+	CORE_DVFS("disp2",   0, KHZ, 158000, 158000, 190000, 190000, 190000),
+	CORE_DVFS("hdmi",    0, KHZ, 0,      0,      0,      148500, 148500),
+
+	/*
+	 * These clocks technically depend on the core process id,
+	 * but just use the worst case value for now
+	 */
+	CORE_DVFS("host1x",  1, KHZ, 104500, 133000, 166000, 166000, 166000),
+	CORE_DVFS("epp",     1, KHZ, 133000, 171000, 247000, 300000, 300000),
+	CORE_DVFS("2d",      1, KHZ, 133000, 171000, 247000, 300000, 300000),
+	CORE_DVFS("3d",      1, KHZ, 114000, 161500, 247000, 300000, 300000),
+	CORE_DVFS("mpe",     1, KHZ, 104500, 152000, 228000, 250000, 250000),
+	CORE_DVFS("vi",      1, KHZ, 85000,  100000, 150000, 150000, 150000),
+	CORE_DVFS("sclk",    1, KHZ, 95000,  133000, 190000, 250000, 250000),
+	CORE_DVFS("vde",     1, KHZ, 95000,  123500, 209000, 250000, 250000),
+	/* What is this? */
+	CORE_DVFS("NVRM_DEVID_CLK_SRC", 1, MHZ, 480, 600, 800, 1067, 1067),
+};
+
+int tegra_dvfs_disable_core_set(const char *arg, const struct kernel_param *kp)
+{
+	int ret;
+
+	ret = param_set_bool(arg, kp);
+	if (ret)
+		return ret;
+
+	if (tegra_dvfs_core_disabled)
+		tegra_dvfs_rail_disable(&tegra2_dvfs_rail_vdd_core);
+	else
+		tegra_dvfs_rail_enable(&tegra2_dvfs_rail_vdd_core);
+
+	return 0;
+}
+
+int tegra_dvfs_disable_cpu_set(const char *arg, const struct kernel_param *kp)
+{
+	int ret;
+
+	ret = param_set_bool(arg, kp);
+	if (ret)
+		return ret;
+
+	if (tegra_dvfs_cpu_disabled)
+		tegra_dvfs_rail_disable(&tegra2_dvfs_rail_vdd_cpu);
+	else
+		tegra_dvfs_rail_enable(&tegra2_dvfs_rail_vdd_cpu);
+
+	return 0;
+}
+
+int tegra_dvfs_disable_get(char *buffer, const struct kernel_param *kp)
+{
+	return param_get_bool(buffer, kp);
+}
+
+static struct kernel_param_ops tegra_dvfs_disable_core_ops = {
+	.set = tegra_dvfs_disable_core_set,
+	.get = tegra_dvfs_disable_get,
+};
+
+static struct kernel_param_ops tegra_dvfs_disable_cpu_ops = {
+	.set = tegra_dvfs_disable_cpu_set,
+	.get = tegra_dvfs_disable_get,
+};
+
+module_param_cb(disable_core, &tegra_dvfs_disable_core_ops,
+	&tegra_dvfs_core_disabled, 0644);
+module_param_cb(disable_cpu, &tegra_dvfs_disable_cpu_ops,
+	&tegra_dvfs_cpu_disabled, 0644);
+
+void __init tegra2_init_dvfs(void)
+{
+	int i;
+	struct clk *c;
+	struct dvfs *d;
+	int ret;
+	int cpu_process_id = tegra_cpu_process_id();
+
+	tegra_dvfs_init_rails(tegra2_dvfs_rails, ARRAY_SIZE(tegra2_dvfs_rails));
+	tegra_dvfs_add_relationships(tegra2_dvfs_relationships,
+		ARRAY_SIZE(tegra2_dvfs_relationships));
+	/*
+	 * VDD_CORE must always be at least 50 mV higher than VDD_CPU
+	 * Fill out cpu_core_millivolts based on cpu_millivolts
+	 */
+	for (i = 0; i < ARRAY_SIZE(dvfs_init); i++) {
+		d = &dvfs_init[i];
+
+		if (d->cpu_process_id != -1 &&
+				d->cpu_process_id != cpu_process_id)
+			continue;
+
+		c = tegra_get_clock_by_name(d->clk_name);
+
+		if (!c) {
+			pr_debug("tegra_dvfs: no clock found for %s\n",
+				d->clk_name);
+			continue;
+		}
+
+		ret = tegra_enable_dvfs_on_clk(c, d);
+		if (ret)
+			pr_err("tegra_dvfs: failed to enable dvfs on %s\n",
+				c->name);
+	}
+
+	if (tegra_dvfs_core_disabled)
+		tegra_dvfs_rail_disable(&tegra2_dvfs_rail_vdd_core);
+
+	if (tegra_dvfs_cpu_disabled)
+		tegra_dvfs_rail_disable(&tegra2_dvfs_rail_vdd_cpu);
+}