ARM: tegra: Add dvfs rails

The previous version of dvfs handled requirements between two
different voltage rails by using two sets of dvfs tables, one
for each rail.  That method fails for vdd_aon, which must be
within 170 mV of vdd_core.

Instead, have each dvfs clock only set the voltage rail that
it directly depends on, and add a relationship system to the
voltage rails.  When the voltage changes on one rail, it calls
update on all the rails that depend on it.  The dependent rails
compare the new voltage of the original rail to their own
voltage, and update their own voltage as necessary.

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

5 files changed, 467 insertions, 282 deletions

diff --git a/arch/arm/mach-tegra/clock.c b/arch/arm/mach-tegra/clock.c
index 34c2c29fa760..124af0f78782 100644
--- a/arch/arm/mach-tegra/clock.c
+++ b/arch/arm/mach-tegra/clock.c
@@ -87,7 +87,7 @@ static inline bool clk_is_auto_dvfs(struct clk *c)
 
 static inline bool clk_is_dvfs(struct clk *c)
 {
-	return c->is_dvfs;
+	return (c->dvfs != NULL);
 }
 
 static inline bool clk_cansleep(struct clk *c)
@@ -217,8 +217,6 @@ void clk_init(struct clk *c)
 {
 	clk_lock_init(c);
 
-	INIT_LIST_HEAD(&c->dvfs);
-
 	if (c->ops && c->ops->init)
 		c->ops->init(c);
 
@@ -511,35 +509,6 @@ void __init tegra_init_clock(void)
 }
 
 /*
- * Iterate through all clocks, setting the dvfs rate to the current clock
- * rate on all auto dvfs clocks, and to the saved dvfs rate on all manual
- * dvfs clocks.  Used to enable dvfs during late init, after the regulators
- * are available.
- */
-void __init tegra_clk_set_dvfs_rates(void)
-{
-	unsigned long flags;
-	struct clk *c;
-
-	mutex_lock(&clock_list_lock);
-
-	list_for_each_entry(c, &clocks, node) {
-		clk_lock_save(c, flags);
-		if (clk_is_auto_dvfs(c)) {
-			if (c->refcnt > 0)
-				tegra_dvfs_set_rate(c, clk_get_rate_locked(c));
-			else
-				tegra_dvfs_set_rate(c, 0);
-		} else if (clk_is_dvfs(c)) {
-			tegra_dvfs_set_rate(c, c->dvfs_rate);
-		}
-		clk_unlock_restore(c, flags);
-	}
-
-	mutex_unlock(&clock_list_lock);
-}
-
-/*
  * Iterate through all clocks, disabling any for which the refcount is 0
  * but the clock init detected the bootloader left the clock on.
  */
@@ -570,7 +539,6 @@ int __init tegra_late_init_clock(void)
 {
 	tegra_dvfs_late_init();
 	tegra_disable_boot_clocks();
-	tegra_clk_set_dvfs_rates();
 	return 0;
 }
 late_initcall(tegra_late_init_clock);
@@ -694,7 +662,7 @@ 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->reg_id,
+			30 - level * 3, d->dvfs_rail->reg_id,
 			"",
 			d->cur_millivolts);
 }
@@ -702,7 +670,6 @@ static void dvfs_show_one(struct seq_file *s, struct dvfs *d, int level)
 static void clock_tree_show_one(struct seq_file *s, struct clk *c, int level)
 {
 	struct clk *child;
-	struct dvfs *d;
 	const char *state = "uninit";
 	char div[8] = {0};
 
@@ -735,8 +702,8 @@ 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));
 
-	list_for_each_entry(d, &c->dvfs, node)
-		dvfs_show_one(s, d, level + 1);
+	if (c->dvfs)
+		dvfs_show_one(s, c->dvfs, level + 1);
 
 	list_for_each_entry(child, &clocks, node) {
 		if (child->parent != c)
diff --git a/arch/arm/mach-tegra/clock.h b/arch/arm/mach-tegra/clock.h
index 42d9bd8849e0..1d6a9acba412 100644
--- a/arch/arm/mach-tegra/clock.h
+++ b/arch/arm/mach-tegra/clock.h
@@ -77,7 +77,7 @@ enum clk_state {
 struct clk {
 	/* node for master clocks list */
 	struct list_head	node;		/* node for list of all clocks */
-	struct list_head	dvfs;		/* list of dvfs dependencies */
+	struct dvfs 		*dvfs;
 	struct clk_lookup	lookup;
 
 #ifdef CONFIG_DEBUG_FS
@@ -90,7 +90,6 @@ struct clk {
 	unsigned long		rate;
 	unsigned long		max_rate;
 	unsigned long		min_rate;
-	bool			is_dvfs;
 	bool			auto_dvfs;
 	bool			cansleep;
 	u32			flags;
@@ -161,7 +160,6 @@ 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 tegra_clk_set_dvfs_rates(void);
 void clk_set_cansleep(struct clk *c);
 unsigned long clk_get_rate_locked(struct clk *c);
 
diff --git a/arch/arm/mach-tegra/dvfs.c b/arch/arm/mach-tegra/dvfs.c
index d7c5032dcbb3..ff48ea0c64f0 100644
--- a/arch/arm/mach-tegra/dvfs.c
+++ b/arch/arm/mach-tegra/dvfs.c
@@ -18,131 +18,198 @@
 
 #include <linux/kernel.h>
 #include <linux/clk.h>
-#include <linux/list.h>
+#include <linux/debugfs.h>
 #include <linux/init.h>
+#include <linux/list.h>
 #include <linux/list_sort.h>
 #include <linux/module.h>
-#include <linux/debugfs.h>
-#include <linux/slab.h>
-#include <linux/seq_file.h>
 #include <linux/regulator/consumer.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+#include <linux/suspend.h>
+#include <linux/delay.h>
+
 #include <asm/clkdev.h>
+
 #include <mach/clk.h>
 
 #include "board.h"
 #include "clock.h"
 #include "dvfs.h"
 
-struct dvfs_reg {
-	struct list_head node;  /* node in dvfs_reg_list */
-	struct list_head dvfs;  /* list head of attached dvfs clocks */
-	const char *reg_id;
-	struct regulator *reg;
-	int max_millivolts;
-	int millivolts;
-	struct mutex lock;
-};
-
-static LIST_HEAD(dvfs_debug_list);
-static LIST_HEAD(dvfs_reg_list);
+static LIST_HEAD(dvfs_rail_list);
+static DEFINE_MUTEX(dvfs_lock);
 
-static DEFINE_MUTEX(dvfs_debug_list_lock);
-static DEFINE_MUTEX(dvfs_reg_list_lock);
+static int dvfs_rail_update(struct dvfs_rail *rail);
 
-static int dvfs_reg_set_voltage(struct dvfs_reg *dvfs_reg)
+void tegra_dvfs_add_relationships(struct dvfs_relationship *rels, int n)
 {
-	int millivolts = 0;
-	struct dvfs *d;
-	int ret = 0;
+	int i;
+	struct dvfs_relationship *rel;
 
-	mutex_lock(&dvfs_reg->lock);
+	mutex_lock(&dvfs_lock);
 
-	list_for_each_entry(d, &dvfs_reg->dvfs, reg_node)
-		millivolts = max(d->cur_millivolts, millivolts);
+	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);
+	}
 
-	if (millivolts == dvfs_reg->millivolts)
-		goto out;
+	mutex_unlock(&dvfs_lock);
+}
 
-	dvfs_reg->millivolts = millivolts;
+int tegra_dvfs_init_rails(struct dvfs_rail *rails[], int n)
+{
+	int i;
 
-	if (!dvfs_reg->reg) {
-		pr_warn("dvfs set voltage on %s ignored\n", dvfs_reg->reg_id);
-		goto out;
+	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);
 	}
 
-	ret = regulator_set_voltage(dvfs_reg->reg,
-		millivolts * 1000, dvfs_reg->max_millivolts * 1000);
+	mutex_unlock(&dvfs_lock);
 
-out:
-	mutex_unlock(&dvfs_reg->lock);
-	return ret;
+	return 0;
+};
+
+static int dvfs_solve_relationship(struct dvfs_relationship *rel)
+{
+	return rel->solve(rel->from, rel->to);
 }
 
-static int dvfs_reg_connect_to_regulator(struct dvfs_reg *dvfs_reg)
+/* 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)
 {
-	struct regulator *reg;
+	int ret = 0;
+	struct dvfs_relationship *rel;
+	int step = (millivolts > rail->millivolts) ? rail->step : -rail->step;
+	int i;
+	int steps;
 
-	if (!dvfs_reg->reg) {
-		reg = regulator_get(NULL, dvfs_reg->reg_id);
-		if (IS_ERR(reg))
+	if (!rail->reg) {
+		if (millivolts == rail->millivolts)
+			return 0;
+		else
 			return -EINVAL;
 	}
 
-	dvfs_reg->reg = reg;
+	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 0;
+	return ret;
 }
 
-static struct dvfs_reg *get_dvfs_reg(struct dvfs *d)
+/* 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)
 {
-	struct dvfs_reg *dvfs_reg;
+	int millivolts = 0;
+	struct dvfs *d;
+	struct dvfs_relationship *rel;
+	int ret = 0;
 
-	mutex_lock(&dvfs_reg_list_lock);
+	/* if dvfs is suspended, return and handle it during resume */
+	if (rail->suspended)
+		return 0;
 
-	list_for_each_entry(dvfs_reg, &dvfs_reg_list, node)
-		if (!strcmp(d->reg_id, dvfs_reg->reg_id))
-			goto out;
+	/* if regulators are not connected yet, return and handle it later */
+	if (!rail->reg)
+		return 0;
 
-	dvfs_reg = kzalloc(sizeof(struct dvfs_reg), GFP_KERNEL);
-	if (!dvfs_reg) {
-		pr_err("%s: Failed to allocate dvfs_reg\n", __func__);
-		goto out;
-	}
+	/* Find the maximum voltage requested by any clock */
+	list_for_each_entry(d, &rail->dvfs, reg_node)
+		millivolts = max(d->cur_millivolts, millivolts);
 
-	mutex_init(&dvfs_reg->lock);
-	INIT_LIST_HEAD(&dvfs_reg->dvfs);
-	dvfs_reg->reg_id = kstrdup(d->reg_id, GFP_KERNEL);
+	rail->new_millivolts = millivolts;
 
-	list_add_tail(&dvfs_reg->node, &dvfs_reg_list);
+	/* 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);
 
-out:
-	mutex_unlock(&dvfs_reg_list_lock);
-	return dvfs_reg;
+	if (rail->new_millivolts != rail->millivolts)
+		ret = dvfs_rail_set_voltage(rail, rail->new_millivolts);
+
+	return ret;
 }
 
-static struct dvfs_reg *attach_dvfs_reg(struct dvfs *d)
+static int dvfs_rail_connect_to_regulator(struct dvfs_rail *rail)
 {
-	struct dvfs_reg *dvfs_reg;
-
-	dvfs_reg = get_dvfs_reg(d);
-	if (!dvfs_reg)
-		return NULL;
-
-	mutex_lock(&dvfs_reg->lock);
-	list_add_tail(&d->reg_node, &dvfs_reg->dvfs);
+	struct regulator *reg;
 
-	d->dvfs_reg = dvfs_reg;
-	if (d->max_millivolts > d->dvfs_reg->max_millivolts)
-		d->dvfs_reg->max_millivolts = d->max_millivolts;
+	if (!rail->reg) {
+		reg = regulator_get(NULL, rail->reg_id);
+		if (IS_ERR(reg))
+			return -EINVAL;
+	}
 
-	d->cur_millivolts = d->max_millivolts;
-	mutex_unlock(&dvfs_reg->lock);
+	rail->reg = reg;
 
-	return dvfs_reg;
+	return 0;
 }
 
 static int
-__tegra_dvfs_set_rate(struct clk *c, struct dvfs *d, unsigned long rate)
+__tegra_dvfs_set_rate(struct dvfs *d, unsigned long rate)
 {
 	int i = 0;
 	int ret;
@@ -152,7 +219,7 @@ __tegra_dvfs_set_rate(struct clk *c, struct dvfs *d, unsigned long rate)
 
 	if (rate > d->freqs[d->num_freqs - 1]) {
 		pr_warn("tegra_dvfs: rate %lu too high for dvfs on %s\n", rate,
-			c->name);
+			d->clk_name);
 		return -EINVAL;
 	}
 
@@ -167,42 +234,26 @@ __tegra_dvfs_set_rate(struct clk *c, struct dvfs *d, unsigned long rate)
 
 	d->cur_rate = rate;
 
-	if (!d->dvfs_reg)
-		return 0;
-
-	ret = dvfs_reg_set_voltage(d->dvfs_reg);
+	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_reg->reg_id, c->name, d->cur_millivolts);
+			d->dvfs_rail->reg_id, d->clk_name, d->cur_millivolts);
 
 	return ret;
 }
 
 int tegra_dvfs_set_rate(struct clk *c, unsigned long rate)
 {
-	struct dvfs *d;
-	int ret = 0;
-	bool freq_up;
+	int ret;
 
-	c->dvfs_rate = rate;
+	if (!c->dvfs)
+		return -EINVAL;
 
-	freq_up = (c->refcnt == 0) || (rate > clk_get_rate_locked(c));
+	mutex_lock(&dvfs_lock);
+	ret = __tegra_dvfs_set_rate(c->dvfs, rate);
+	mutex_unlock(&dvfs_lock);
 
-	list_for_each_entry(d, &c->dvfs, node) {
-		if (d->higher == freq_up)
-			ret = __tegra_dvfs_set_rate(c, d, rate);
-		if (ret)
-			return ret;
-	}
-
-	list_for_each_entry(d, &c->dvfs, node) {
-		if (d->higher != freq_up)
-			ret = __tegra_dvfs_set_rate(c, d, rate);
-		if (ret)
-			return ret;
-	}
-
-	return 0;
+	return ret;
 }
 EXPORT_SYMBOL(tegra_dvfs_set_rate);
 
@@ -210,12 +261,12 @@ EXPORT_SYMBOL(tegra_dvfs_set_rate);
 int __init tegra_enable_dvfs_on_clk(struct clk *c, struct dvfs *d)
 {
 	int i;
-	struct dvfs_reg *dvfs_reg;
 
-	dvfs_reg = attach_dvfs_reg(d);
-	if (!dvfs_reg) {
-		pr_err("Failed to get regulator %s for clock %s\n",
-			d->reg_id, c->name);
+	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;
 	}
 
@@ -236,17 +287,114 @@ int __init tegra_enable_dvfs_on_clk(struct clk *c, struct dvfs *d)
 		clk_set_cansleep(c);
 	}
 
-	c->is_dvfs = true;
+	c->dvfs = d;
 
-	list_add_tail(&d->node, &c->dvfs);
-
-	mutex_lock(&dvfs_debug_list_lock);
-	list_add_tail(&d->debug_node, &dvfs_debug_list);
-	mutex_unlock(&dvfs_debug_list_lock);
+	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,
+};
+
 /*
  * Iterate through all the dvfs regulators, finding the regulator exported
  * by the regulator api for each one.  Must be called in late init, after
@@ -254,12 +402,19 @@ int __init tegra_enable_dvfs_on_clk(struct clk *c, struct dvfs *d)
  */
 int __init tegra_dvfs_late_init(void)
 {
-	struct dvfs_reg *dvfs_reg;
+	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_lock(&dvfs_reg_list_lock);
-	list_for_each_entry(dvfs_reg, &dvfs_reg_list, node)
-		dvfs_reg_connect_to_regulator(dvfs_reg);
-	mutex_unlock(&dvfs_reg_list_lock);
+	mutex_unlock(&dvfs_lock);
+
+	register_pm_notifier(&tegra_dvfs_nb);
 
 	return 0;
 }
@@ -267,11 +422,11 @@ int __init tegra_dvfs_late_init(void)
 #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, debug_node);
-	struct dvfs *db = list_entry(b, struct dvfs, debug_node);
+	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->reg_id, db->reg_id);
+	ret = strcmp(da->dvfs_rail->reg_id, db->dvfs_rail->reg_id);
 	if (ret != 0)
 		return ret;
 
@@ -286,27 +441,33 @@ static int dvfs_tree_sort_cmp(void *p, struct list_head *a, struct list_head *b)
 static int dvfs_tree_show(struct seq_file *s, void *data)
 {
 	struct dvfs *d;
-	const char *last_reg = "";
+	struct dvfs_rail *rail;
+	struct dvfs_relationship *rel;
 
 	seq_printf(s, "   clock      rate       mV\n");
 	seq_printf(s, "--------------------------------\n");
 
-	mutex_lock(&dvfs_debug_list_lock);
-
-	list_sort(NULL, &dvfs_debug_list, dvfs_tree_sort_cmp);
+	mutex_lock(&dvfs_lock);
 
-	list_for_each_entry(d, &dvfs_debug_list, debug_node) {
-		if (strcmp(last_reg, d->dvfs_reg->reg_id) != 0) {
-			last_reg = d->dvfs_reg->reg_id;
-			seq_printf(s, "%s %d mV:\n", d->dvfs_reg->reg_id,
-				d->dvfs_reg->millivolts);
+	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));
 		}
 
-		seq_printf(s, "   %-10s %-10lu %-4d mV\n", d->clk_name,
-			d->cur_rate, d->cur_millivolts);
+		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_debug_list_lock);
+	mutex_unlock(&dvfs_lock);
 
 	return 0;
 }
diff --git a/arch/arm/mach-tegra/dvfs.h b/arch/arm/mach-tegra/dvfs.h
index e5eac6cf9cd0..85764580c39d 100644
--- a/arch/arm/mach-tegra/dvfs.h
+++ b/arch/arm/mach-tegra/dvfs.h
@@ -22,25 +22,57 @@
 #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 process_id;
-	bool cpu;
+	int cpu_process_id;
 
 	/* Must be initialized before tegra_dvfs_init */
-	const char *reg_id;
 	int freqs_mult;
 	unsigned long freqs[MAX_DVFS_FREQS];
-	unsigned long millivolts[MAX_DVFS_FREQS];
+	const int *millivolts;
+	struct dvfs_rail *dvfs_rail;
 	bool auto_dvfs;
-	bool higher;
 
 	/* Filled in by tegra_dvfs_init */
 	int max_millivolts;
 	int num_freqs;
-	struct dvfs_reg *dvfs_reg;
 
 	int cur_millivolts;
 	unsigned long cur_rate;
@@ -53,5 +85,7 @@ 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);
 
 #endif
diff --git a/arch/arm/mach-tegra/tegra2_dvfs.c b/arch/arm/mach-tegra/tegra2_dvfs.c
index 265a7b538f7f..1734e5ee700b 100644
--- a/arch/arm/mach-tegra/tegra2_dvfs.c
+++ b/arch/arm/mach-tegra/tegra2_dvfs.c
@@ -25,81 +25,123 @@
 #include "dvfs.h"
 #include "fuse.h"
 
-#define CORE_REGULATOR "vdd_core"
-#define CPU_REGULATOR "vdd_cpu"
-
 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};
-static int cpu_core_millivolts[MAX_DVFS_FREQS];
-
-#define CORE_MAX_MILLIVOLTS 1275
-#define CPU_MAX_MILLIVOLTS 1100
 
 #define KHZ 1000
 #define MHZ 1000000
 
-#ifdef CONFIG_TEGRA_CPU_DVFS
-#define CPU_DVFS_CPU(_clk_name, _process_id, _mult, _freqs...)	\
+static struct dvfs_rail tegra2_dvfs_rail_vdd_cpu = {
+	.reg_id = "vdd_cpu",
+	.max_millivolts = 1100,
+	.min_millivolts = 750,
+	.nominal_millivolts = 1100,
+#ifndef CONFIG_TEGRA_CPU_DVFS
+	.disabled = true,
+#endif
+};
+
+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 */
+#ifndef CONFIG_TEGRA_CORE_DVFS
+	.disabled = true,
+#endif
+};
+
+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,			\
-		.reg_id		= CPU_REGULATOR,		\
-		.cpu		= true,				\
-		.process_id	= _process_id,			\
+		.cpu_process_id	= _process_id,			\
 		.freqs		= {_freqs},			\
 		.freqs_mult	= _mult,			\
+		.millivolts	= cpu_millivolts,		\
 		.auto_dvfs	= true,				\
-		.max_millivolts = CPU_MAX_MILLIVOLTS    	\
-	},
+		.dvfs_rail	= &tegra2_dvfs_rail_vdd_cpu,	\
+	}
 
-#ifdef CONFIG_TEGRA_CORE_DVFS /* CPU_DVFS && CORE_DVFS */
-#define CPU_DVFS_CORE(_clk_name, _process_id, _mult, _freqs...)	\
+#define CORE_DVFS(_clk_name, _auto, _mult, _freqs...)		\
 	{							\
 		.clk_name	= _clk_name,			\
-		.reg_id		= CORE_REGULATOR,		\
-		.cpu		= false,			\
-		.process_id	= _process_id,			\
+		.cpu_process_id = -1,				\
 		.freqs		= {_freqs},			\
 		.freqs_mult	= _mult,			\
-		.auto_dvfs	= true,				\
-		.higher		= true,				\
-		.max_millivolts = CORE_MAX_MILLIVOLTS   	\
-	},
-#else /* CPU_DVFS && !CORE_DVFS */
-#define CPU_DVFS_CORE(_clk_name, _process_id, _mult, _freqs...)
-#endif
-#else /* !CPU_DVFS */
-#define CPU_DVFS_CPU(_clk_name, _process_id, _mult, _freqs...)
-#define CPU_DVFS_CORE(_clk_name, _process_id, _mult, _freqs...)
-#endif
-
-#ifdef CONFIG_TEGRA_CORE_DVFS
-#define CORE_DVFS(_clk_name, _auto, _mult, _freqs...)	\
-	{						\
-		.clk_name	= _clk_name,		\
-		.reg_id		= CORE_REGULATOR,	\
-		.process_id	= -1,			\
-		.freqs		= {_freqs},		\
-		.freqs_mult	= _mult,		\
-		.auto_dvfs	= _auto,		\
-		.max_millivolts = CORE_MAX_MILLIVOLTS   \
-	},
-#else
-#define CORE_DVFS(_clk_name, _process_id, _mult, _freqs...)
-#endif
-
-#define CPU_DVFS(_clk_name, _process_id, _mult, _freqs...)	\
-	CPU_DVFS_CORE(_clk_name, _process_id, _mult, _freqs)	\
-	CPU_DVFS_CPU(_clk_name, _process_id, _mult, _freqs)	\
-
+		.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)
+	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 */
 
@@ -110,22 +152,22 @@ static struct dvfs dvfs_init[] = {
 	 * 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)
+	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,      480000, 480000, 480000)
-	CORE_DVFS("usb2",    1, KHZ, 0,      0,      480000, 480000, 480000)
-	CORE_DVFS("usb3",    1, KHZ, 0,      0,      480000, 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)
+	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,      480000, 480000, 480000),
+	CORE_DVFS("usb2",    1, KHZ, 0,      0,      480000, 480000, 480000),
+	CORE_DVFS("usb3",    1, KHZ, 0,      0,      480000, 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
@@ -133,24 +175,24 @@ static struct dvfs dvfs_init[] = {
 	 * 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)
+	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)
+	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)
+	CORE_DVFS("NVRM_DEVID_CLK_SRC", 1, MHZ, 480, 600, 800, 1067, 1067),
 };
 
 void __init tegra2_init_dvfs(void)
@@ -158,29 +200,22 @@ void __init tegra2_init_dvfs(void)
 	int i;
 	struct clk *c;
 	struct dvfs *d;
-	int process_id;
 	int ret;
-
 	int cpu_process_id = tegra_cpu_process_id();
-	int core_process_id = tegra_core_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(cpu_millivolts); i++)
-		if (cpu_millivolts[i])
-			cpu_core_millivolts[i] = cpu_millivolts[i] + 50;
-
 	for (i = 0; i < ARRAY_SIZE(dvfs_init); i++) {
 		d = &dvfs_init[i];
 
-		process_id = d->cpu ? cpu_process_id : core_process_id;
-		if (d->process_id != -1 && d->process_id != process_id) {
-			pr_debug("tegra_dvfs: rejected %s %d, process_id %d\n",
-				d->clk_name, d->process_id, process_id);
+		if (d->cpu_process_id != -1 &&
+				d->cpu_process_id != cpu_process_id)
 			continue;
-		}
 
 		c = tegra_get_clock_by_name(d->clk_name);
 
@@ -190,16 +225,6 @@ void __init tegra2_init_dvfs(void)
 			continue;
 		}
 
-		if (d->cpu)
-			memcpy(d->millivolts, cpu_millivolts,
-				sizeof(cpu_millivolts));
-		else if (!strcmp(d->clk_name, "cpu"))
-			memcpy(d->millivolts, cpu_core_millivolts,
-				sizeof(cpu_core_millivolts));
-		else
-			memcpy(d->millivolts, core_millivolts,
-				sizeof(core_millivolts));
-
 		ret = tegra_enable_dvfs_on_clk(c, d);
 		if (ret)
 			pr_err("tegra_dvfs: failed to enable dvfs on %s\n",