merging android-tegra-2.6.36 into git-master/linux-2.6/android-tegra-2.6.36

Change-Id: I1312ec33ba8bac38dc395d7d1a2f485b13d74c14

17 files changed, 1105 insertions, 345 deletions

diff --git a/arch/arm/mach-tegra/Kconfig b/arch/arm/mach-tegra/Kconfig
index ef028cd62734..8269eea7ee77 100644
--- a/arch/arm/mach-tegra/Kconfig
+++ b/arch/arm/mach-tegra/Kconfig
@@ -75,6 +75,10 @@ config TEGRA_FIQ_DEBUGGER
 
 endif
 
+config TEGRA_EMC_SCALING_ENABLE
+	bool "Enable scaling the memory frequency"
+	default n
+
 config TEGRA_CPU_DVFS
 	bool "Enable voltage scaling on Tegra CPU"
 	default y
diff --git a/arch/arm/mach-tegra/Makefile b/arch/arm/mach-tegra/Makefile
index 431ecbb659dc..9262acde3f0c 100644
--- a/arch/arm/mach-tegra/Makefile
+++ b/arch/arm/mach-tegra/Makefile
@@ -28,9 +28,11 @@ 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)         += suspend-t2.o
-obj-$(CONFIG_ARCH_TEGRA_2x_SOC)         += tegra2_save.o
-obj-$(CONFIG_CPU_V7)                    += cortex-a9.o
-obj-$(CONFIG_ARCH_TEGRA_2x_SOC)         += pinmux-t2-tables.o
+obj-$(CONFIG_ARCH_TEGRA_2x_SOC)		+= tegra2_save.o
+obj-$(CONFIG_ARCH_TEGRA_2x_SOC)		+= tegra2_emc.o
+obj-$(CONFIG_CPU_V7)			+= cortex-a9.o
+
+obj-$(CONFIG_ARCH_TEGRA_2x_SOC)		+= pinmux-t2-tables.o
 obj-$(CONFIG_SMP)                       += localtimer.o
 obj-$(CONFIG_SMP)                       += platsmp.o
 obj-y                                   += headsmp.o
diff --git a/arch/arm/mach-tegra/board-ventana.c b/arch/arm/mach-tegra/board-ventana.c
index ff0a81d93fee..2f22bea260d9 100644
--- a/arch/arm/mach-tegra/board-ventana.c
+++ b/arch/arm/mach-tegra/board-ventana.c
@@ -285,7 +285,7 @@ static void ventana_i2c_init(void)
 	}
 
 static struct gpio_keys_button ventana_keys[] = {
-	[0] = GPIO_KEY(KEY_MENU, PQ0, 0),
+	[0] = GPIO_KEY(KEY_MENU, PQ3, 0),
 	[1] = GPIO_KEY(KEY_HOME, PQ1, 0),
 	[2] = GPIO_KEY(KEY_BACK, PQ2, 0),
 	[3] = GPIO_KEY(KEY_VOLUMEUP, PQ5, 0),
diff --git a/arch/arm/mach-tegra/board.h b/arch/arm/mach-tegra/board.h
index c47a21f71260..04f1538b1a37 100644
--- a/arch/arm/mach-tegra/board.h
+++ b/arch/arm/mach-tegra/board.h
@@ -32,6 +32,7 @@ void __init tegra_reserve(unsigned long carveout_size, unsigned long fb_size,
 void __init tegra_protected_aperture_init(unsigned long aperture);
 void tegra_move_framebuffer(unsigned long to, unsigned long from,
 	unsigned long size);
+int tegra_dvfs_rail_disable_by_name(const char *reg_id);
 
 extern unsigned long tegra_bootloader_fb_start;
 extern unsigned long tegra_bootloader_fb_size;
diff --git a/arch/arm/mach-tegra/clock.c b/arch/arm/mach-tegra/clock.c
index ad5f483af7fc..e3936af38356 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)
@@ -207,22 +207,6 @@ void clk_set_cansleep(struct clk *c)
 	mutex_unlock(&clock_list_lock);
 }
 
-int tegra_dvfs_set_rate(struct clk *c, unsigned long rate)
-{
-	unsigned long flags;
-	int ret;
-
-	if (!clk_is_dvfs(c))
-		return -EINVAL;
-
-	clk_lock_save(c, flags);
-	ret = tegra_dvfs_set_rate_locked(c, rate);
-	clk_unlock_restore(c, flags);
-
-	return ret;
-}
-EXPORT_SYMBOL(tegra_dvfs_set_rate);
-
 int clk_reparent(struct clk *c, struct clk *parent)
 {
 	c->parent = parent;
@@ -233,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);
 
@@ -260,7 +242,7 @@ int clk_enable(struct clk *c)
 	clk_lock_save(c, flags);
 
 	if (clk_is_auto_dvfs(c)) {
-		ret = tegra_dvfs_set_rate_locked(c, clk_get_rate_locked(c));
+		ret = tegra_dvfs_set_rate(c, clk_get_rate_locked(c));
 		if (ret)
 			goto out;
 	}
@@ -313,7 +295,7 @@ void clk_disable(struct clk *c)
 	c->refcnt--;
 
 	if (clk_is_auto_dvfs(c) && c->refcnt == 0)
-		tegra_dvfs_set_rate_locked(c, 0);
+		tegra_dvfs_set_rate(c, 0);
 
 	clk_unlock_restore(c, flags);
 }
@@ -338,7 +320,7 @@ int clk_set_parent(struct clk *c, struct clk *parent)
 
 	if (clk_is_auto_dvfs(c) && c->refcnt > 0 &&
 			(!c->parent || new_rate > old_rate)) {
-		ret = tegra_dvfs_set_rate_locked(c, new_rate);
+		ret = tegra_dvfs_set_rate(c, new_rate);
 		if (ret)
 			goto out;
 	}
@@ -349,7 +331,7 @@ int clk_set_parent(struct clk *c, struct clk *parent)
 
 	if (clk_is_auto_dvfs(c) && c->refcnt > 0 &&
 			new_rate < old_rate)
-		ret = tegra_dvfs_set_rate_locked(c, new_rate);
+		ret = tegra_dvfs_set_rate(c, new_rate);
 
 out:
 	clk_unlock_restore(c, flags);
@@ -368,6 +350,7 @@ int clk_set_rate(struct clk *c, unsigned long rate)
 	int ret = 0;
 	unsigned long flags;
 	unsigned long old_rate;
+	long new_rate;
 
 	clk_lock_save(c, flags);
 
@@ -381,8 +364,19 @@ int clk_set_rate(struct clk *c, unsigned long rate)
 	if (rate > c->max_rate)
 		rate = c->max_rate;
 
+	if (c->ops && c->ops->round_rate) {
+		new_rate = c->ops->round_rate(c, rate);
+
+		if (new_rate < 0) {
+			ret = new_rate;
+			goto out;
+		}
+
+		rate = new_rate;
+	}
+
 	if (clk_is_auto_dvfs(c) && rate > old_rate && c->refcnt > 0) {
-		ret = tegra_dvfs_set_rate_locked(c, rate);
+		ret = tegra_dvfs_set_rate(c, rate);
 		if (ret)
 			goto out;
 	}
@@ -392,7 +386,7 @@ int clk_set_rate(struct clk *c, unsigned long rate)
 		goto out;
 
 	if (clk_is_auto_dvfs(c) && rate < old_rate && c->refcnt > 0)
-		ret = tegra_dvfs_set_rate_locked(c, rate);
+		ret = tegra_dvfs_set_rate(c, rate);
 
 out:
 	clk_unlock_restore(c, flags);
@@ -527,36 +521,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_locked(c,
-					clk_get_rate_locked(c));
-			else
-				tegra_dvfs_set_rate_locked(c, 0);
-		} else if (clk_is_dvfs(c)) {
-			tegra_dvfs_set_rate_locked(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.
  */
@@ -587,7 +551,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);
@@ -711,7 +674,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);
 }
@@ -719,7 +682,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};
 
@@ -752,8 +714,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 083815487c17..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
@@ -89,7 +89,7 @@ struct clk {
 	unsigned long		dvfs_rate;
 	unsigned long		rate;
 	unsigned long		max_rate;
-	bool			is_dvfs;
+	unsigned long		min_rate;
 	bool			auto_dvfs;
 	bool			cansleep;
 	u32			flags;
@@ -105,6 +105,8 @@ struct clk {
 	u32				reg;
 	u32				reg_shift;
 
+	struct list_head		shared_bus_list;
+
 	union {
 		struct {
 			unsigned int			clk_num;
@@ -128,10 +130,6 @@ struct clk {
 			struct clk			*backup;
 		} cpu;
 		struct {
-			struct list_head		list;
-			unsigned long			min_rate;
-		} shared_bus;
-		struct {
 			struct list_head		node;
 			bool				enabled;
 			unsigned long			rate;
@@ -162,9 +160,7 @@ 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);
-int tegra_dvfs_set_rate_locked(struct clk *c, unsigned long rate);
 
 #endif
diff --git a/arch/arm/mach-tegra/cpu-tegra.c b/arch/arm/mach-tegra/cpu-tegra.c
index 035e1bc31462..d8c103e8964c 100644
--- a/arch/arm/mach-tegra/cpu-tegra.c
+++ b/arch/arm/mach-tegra/cpu-tegra.c
@@ -29,6 +29,7 @@
 #include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/suspend.h>
+#include <linux/debugfs.h>
 
 #include <asm/smp_twd.h>
 #include <asm/system.h>
@@ -36,7 +37,7 @@
 #include <mach/hardware.h>
 #include <mach/clk.h>
 
-/* Frequency table index must be sequential starting at 0 */
+/* Frequency table index must be sequential starting at 0 and frequencies must be ascending*/
 static struct cpufreq_frequency_table freq_table[] = {
 	{ 0, 216000 },
 	{ 1, 312000 },
@@ -49,14 +50,28 @@ static struct cpufreq_frequency_table freq_table[] = {
 	{ 8, CPUFREQ_TABLE_END },
 };
 
+/* CPU frequency is gradually lowered when throttling is enabled */
+#define THROTTLE_START_INDEX	2
+#define THROTTLE_END_INDEX	6
+#define THROTTLE_DELAY		msecs_to_jiffies(2000)
+#define NO_DELAY		msecs_to_jiffies(0)
+
 #define NUM_CPUS	2
 
 static struct clk *cpu_clk;
+static struct clk *emc_clk;
+
+static struct workqueue_struct *workqueue;
 
 static unsigned long target_cpu_speed[NUM_CPUS];
 static DEFINE_MUTEX(tegra_cpu_lock);
 static bool is_suspended;
 
+static DEFINE_MUTEX(throttling_lock);
+static bool is_throttling;
+static struct delayed_work throttle_work;
+
+
 int tegra_verify_speed(struct cpufreq_policy *policy)
 {
 	return cpufreq_frequency_table_verify(policy, freq_table);
@@ -101,6 +116,17 @@ static int tegra_update_cpu_speed(unsigned long rate)
 	if (freqs.old == freqs.new)
 		return ret;
 
+	/*
+	 * Vote on memory bus frequency based on cpu frequency
+	 * This sets the minimum frequency, display or avp may request higher
+	 */
+	if (rate >= 816000)
+		clk_set_rate(emc_clk, 600000000); /* cpu 816 MHz, emc max */
+	else if (rate >= 456000)
+		clk_set_rate(emc_clk, 300000000); /* cpu 456 MHz, emc 150Mhz */
+	else
+		clk_set_rate(emc_clk, 100000000);  /* emc 50Mhz */
+
 	for_each_online_cpu(freqs.cpu)
 		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
 
@@ -143,6 +169,8 @@ static int tegra_target(struct cpufreq_policy *policy,
 {
 	int idx;
 	unsigned int freq;
+	unsigned int highest_speed;
+	unsigned int limit_when_throttling;
 	int ret = 0;
 
 	mutex_lock(&tegra_cpu_lock);
@@ -159,13 +187,137 @@ static int tegra_target(struct cpufreq_policy *policy,
 
 	target_cpu_speed[policy->cpu] = freq;
 
-	ret = tegra_update_cpu_speed(tegra_cpu_highest_speed());
+	highest_speed = tegra_cpu_highest_speed();
 
+	/* Do not go above this frequency when throttling */
+	limit_when_throttling = freq_table[THROTTLE_START_INDEX].frequency;
+
+	if (is_throttling && highest_speed > limit_when_throttling) {
+		if (tegra_getspeed(0) < limit_when_throttling) {
+			ret = tegra_update_cpu_speed(limit_when_throttling);
+			goto out;
+		} else {
+			ret = -EBUSY;
+			goto out;
+		}
+	}
+
+	ret = tegra_update_cpu_speed(highest_speed);
 out:
 	mutex_unlock(&tegra_cpu_lock);
 	return ret;
 }
 
+static bool tegra_throttling_needed(unsigned long *rate)
+{
+	unsigned int current_freq = tegra_getspeed(0);
+	int i;
+
+	for (i = THROTTLE_END_INDEX; i >= THROTTLE_START_INDEX; i--) {
+		if (freq_table[i].frequency < current_freq) {
+			*rate = freq_table[i].frequency;
+			return true;
+		}
+	}
+
+	return false;
+}
+
+static void tegra_throttle_work_func(struct work_struct *work)
+{
+	unsigned long rate;
+
+	mutex_lock(&tegra_cpu_lock);
+
+	if (tegra_throttling_needed(&rate) && tegra_update_cpu_speed(rate) == 0) {
+		queue_delayed_work(workqueue, &throttle_work, THROTTLE_DELAY);
+	}
+
+	mutex_unlock(&tegra_cpu_lock);
+}
+
+/**
+ * tegra_throttling_enable
+ * This functions may sleep
+ */
+void tegra_throttling_enable(void)
+{
+	mutex_lock(&throttling_lock);
+
+	if (!is_throttling) {
+		is_throttling = true;
+		queue_delayed_work(workqueue, &throttle_work, NO_DELAY);
+	}
+
+	mutex_unlock(&throttling_lock);
+}
+EXPORT_SYMBOL_GPL(tegra_throttling_enable);
+
+/**
+ * tegra_throttling_disable
+ * This functions may sleep
+ */
+void tegra_throttling_disable(void)
+{
+	mutex_lock(&throttling_lock);
+
+	if (is_throttling) {
+		cancel_delayed_work_sync(&throttle_work);
+		is_throttling = false;
+	}
+
+	mutex_unlock(&throttling_lock);
+}
+EXPORT_SYMBOL_GPL(tegra_throttling_disable);
+
+#ifdef CONFIG_DEBUG_FS
+static int throttle_debug_set(void *data, u64 val)
+{
+	if (val) {
+		tegra_throttling_enable();
+	} else {
+		tegra_throttling_disable();
+	}
+
+	return 0;
+}
+static int throttle_debug_get(void *data, u64 *val)
+{
+	*val = (u64) is_throttling;
+	return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(throttle_fops, throttle_debug_get, throttle_debug_set, "%llu\n");
+
+static struct dentry *cpu_tegra_debugfs_root;
+
+static int __init tegra_cpu_debug_init(void)
+{
+	cpu_tegra_debugfs_root = debugfs_create_dir("cpu-tegra", 0);
+
+	if (!cpu_tegra_debugfs_root)
+		return -ENOMEM;
+
+	if (!debugfs_create_file("throttle", 0644, cpu_tegra_debugfs_root, NULL, &throttle_fops))
+		goto err_out;
+
+	return 0;
+
+err_out:
+	debugfs_remove_recursive(cpu_tegra_debugfs_root);
+	return -ENOMEM;
+
+}
+
+static void __exit tegra_cpu_debug_exit(void)
+{
+	debugfs_remove_recursive(cpu_tegra_debugfs_root);
+}
+
+late_initcall(tegra_cpu_debug_init);
+module_exit(tegra_cpu_debug_exit);
+#endif
+
 static int tegra_pm_notify(struct notifier_block *nb, unsigned long event,
 	void *dummy)
 {
@@ -196,6 +348,13 @@ static int tegra_cpu_init(struct cpufreq_policy *policy)
 	if (IS_ERR(cpu_clk))
 		return PTR_ERR(cpu_clk);
 
+	emc_clk = clk_get_sys("cpu", "emc");
+	if (IS_ERR(emc_clk)) {
+		clk_put(cpu_clk);
+		return PTR_ERR(emc_clk);
+	}
+	clk_enable(emc_clk);
+
 	cpufreq_frequency_table_cpuinfo(policy, freq_table);
 	cpufreq_frequency_table_get_attr(freq_table, policy->cpu);
 	policy->cur = tegra_getspeed(policy->cpu);
@@ -207,8 +366,10 @@ static int tegra_cpu_init(struct cpufreq_policy *policy)
 	policy->shared_type = CPUFREQ_SHARED_TYPE_ALL;
 	cpumask_copy(policy->related_cpus, cpu_possible_mask);
 
-	if (policy->cpu == 0)
+	if (policy->cpu == 0) {
+		INIT_DELAYED_WORK(&throttle_work, tegra_throttle_work_func);
 		register_pm_notifier(&tegra_cpu_pm_notifier);
+	}
 
 	return 0;
 }
@@ -216,6 +377,8 @@ static int tegra_cpu_init(struct cpufreq_policy *policy)
 static int tegra_cpu_exit(struct cpufreq_policy *policy)
 {
 	cpufreq_frequency_table_cpuinfo(policy, freq_table);
+	clk_disable(emc_clk);
+	clk_put(emc_clk);
 	clk_put(cpu_clk);
 	return 0;
 }
@@ -237,11 +400,15 @@ static struct cpufreq_driver tegra_cpufreq_driver = {
 
 static int __init tegra_cpufreq_init(void)
 {
+	workqueue = create_singlethread_workqueue("cpu-tegra");
+	if (!workqueue)
+		return -ENOMEM;
 	return cpufreq_register_driver(&tegra_cpufreq_driver);
 }
 
 static void __exit tegra_cpufreq_exit(void)
 {
+	destroy_workqueue(workqueue);
         cpufreq_unregister_driver(&tegra_cpufreq_driver);
 }
 
diff --git a/arch/arm/mach-tegra/dma.c b/arch/arm/mach-tegra/dma.c
index 001dbcc14bfc..484a39182d62 100755
--- a/arch/arm/mach-tegra/dma.c
+++ b/arch/arm/mach-tegra/dma.c
@@ -752,7 +752,7 @@ static void handle_continuous_sngl_dma(struct tegra_dma_channel *ch)
 		 */
 		next_req = list_entry(req->node.next, typeof(*next_req), node);
 		if (next_req->status != TEGRA_DMA_REQ_INFLIGHT) {
-			pr_warning("%s: interrupt during enqueue\n", __func__);
+			pr_debug("%s: interrupt during enqueue\n", __func__);
 			tegra_dma_stop(ch);
 			tegra_dma_update_hw(ch, next_req);
 		} else if (!list_is_last(&next_req->node, &ch->list)) {
diff --git a/arch/arm/mach-tegra/dvfs.c b/arch/arm/mach-tegra/dvfs.c
index ef58fae8afbd..bc1e1a391b5a 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);
+	}
+
+	mutex_unlock(&dvfs_lock);
+}
 
-	if (millivolts == dvfs_reg->millivolts)
-		goto out;
+int tegra_dvfs_init_rails(struct dvfs_rail *rails[], int n)
+{
+	int i;
+
+	mutex_lock(&dvfs_lock);
 
-	dvfs_reg->millivolts = millivolts;
+	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;
 
-	if (!dvfs_reg->reg) {
-		pr_warn("dvfs set voltage on %s ignored\n", dvfs_reg->reg_id);
-		goto out;
+		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;
 
-	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;
 }
 
-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,54 +234,39 @@ __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_locked(struct clk *c, unsigned long rate)
+int tegra_dvfs_set_rate(struct clk *c, unsigned long rate)
 {
-	struct dvfs *d;
-	int ret = 0;
-	bool freq_up;
-
-	c->dvfs_rate = rate;
-
-	freq_up = (c->refcnt == 0) || (rate > clk_get_rate_locked(c));
+	int 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;
-	}
+	if (!c->dvfs)
+		return -EINVAL;
 
-	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;
-	}
+	mutex_lock(&dvfs_lock);
+	ret = __tegra_dvfs_set_rate(c->dvfs, rate);
+	mutex_unlock(&dvfs_lock);
 
-	return 0;
+	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;
-	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;
 	}
 
@@ -235,17 +287,172 @@ 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,
+};
+
+/* 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
@@ -253,12 +460,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);
 
-	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);
+	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;
 }
@@ -266,11 +480,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;
 
@@ -285,27 +499,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..68622b899c59 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,9 @@ 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/suspend.c b/arch/arm/mach-tegra/suspend.c
index af3252401256..146b93a014f3 100644
--- a/arch/arm/mach-tegra/suspend.c
+++ b/arch/arm/mach-tegra/suspend.c
@@ -458,13 +458,17 @@ static void tegra_suspend_dram(bool do_lp0)
 
 	suspend_cpu_complex();
 	flush_cache_all();
+#ifdef CONFIG_CACHE_L2X0
 	l2x0_shutdown();
+#endif
 
 	__cortex_a9_save(mode);
 	restore_cpu_complex();
 
 	writel(orig, evp_reset);
+#ifdef CONFIG_CACHE_L2X0
 	l2x0_restart();
+#endif
 
 	if (!do_lp0) {
 		memcpy(iram_code, iram_save, iram_save_size);
diff --git a/arch/arm/mach-tegra/tegra2_clocks.c b/arch/arm/mach-tegra/tegra2_clocks.c
index c6fca17a28be..eef598e456f1 100644
--- a/arch/arm/mach-tegra/tegra2_clocks.c
+++ b/arch/arm/mach-tegra/tegra2_clocks.c
@@ -32,6 +32,7 @@
 
 #include "clock.h"
 #include "fuse.h"
+#include "tegra2_emc.h"
 
 #define RST_DEVICES			0x004
 #define RST_DEVICES_SET			0x300
@@ -302,8 +303,6 @@ static void tegra2_super_clk_init(struct clk *c)
 	}
 	BUG_ON(sel->input == NULL);
 	c->parent = sel->input;
-
-	INIT_LIST_HEAD(&c->u.shared_bus.list);
 }
 
 static int tegra2_super_clk_enable(struct clk *c)
@@ -1023,6 +1022,53 @@ static struct clk_ops tegra_periph_clk_ops = {
 	.reset			= &tegra2_periph_clk_reset,
 };
 
+/* External memory controller clock ops */
+static void tegra2_emc_clk_init(struct clk *c)
+{
+	tegra2_periph_clk_init(c);
+	c->max_rate = clk_get_rate_locked(c);
+}
+
+static long tegra2_emc_clk_round_rate(struct clk *c, unsigned long rate)
+{
+	long new_rate = rate;
+
+	new_rate = tegra_emc_round_rate(new_rate);
+	if (new_rate < 0)
+		return c->max_rate;
+
+	BUG_ON(new_rate != tegra2_periph_clk_round_rate(c, new_rate));
+
+	return new_rate;
+}
+
+static int tegra2_emc_clk_set_rate(struct clk *c, unsigned long rate)
+{
+	int ret;
+	/* The Tegra2 memory controller has an interlock with the clock
+	 * block that allows memory shadowed registers to be updated,
+	 * and then transfer them to the main registers at the same
+	 * time as the clock update without glitches. */
+	ret = tegra_emc_set_rate(rate);
+	if (ret < 0)
+		return ret;
+
+	ret = tegra2_periph_clk_set_rate(c, rate);
+	udelay(1);
+
+	return ret;
+}
+
+static struct clk_ops tegra_emc_clk_ops = {
+	.init			= &tegra2_emc_clk_init,
+	.enable			= &tegra2_periph_clk_enable,
+	.disable		= &tegra2_periph_clk_disable,
+	.set_parent		= &tegra2_periph_clk_set_parent,
+	.set_rate		= &tegra2_emc_clk_set_rate,
+	.round_rate		= &tegra2_emc_clk_round_rate,
+	.reset			= &tegra2_periph_clk_reset,
+};
+
 /* Clock doubler ops */
 static void tegra2_clk_double_init(struct clk *c)
 {
@@ -1151,15 +1197,16 @@ static struct clk_ops tegra_cdev_clk_ops = {
 static void tegra_clk_shared_bus_update(struct clk *bus)
 {
 	struct clk *c;
-	unsigned long rate = bus->u.shared_bus.min_rate;
+	unsigned long rate = bus->min_rate;
 
-	list_for_each_entry(c, &bus->u.shared_bus.list,
+	list_for_each_entry(c, &bus->shared_bus_list,
 			u.shared_bus_user.node) {
 		if (c->u.shared_bus_user.enabled)
 			rate = max(c->u.shared_bus_user.rate, rate);
 	}
 
-	clk_set_rate(bus, rate);
+	if (rate != clk_get_rate(bus))
+		clk_set_rate(bus, rate);
 };
 
 static void tegra_clk_shared_bus_init(struct clk *c)
@@ -1170,7 +1217,7 @@ static void tegra_clk_shared_bus_init(struct clk *c)
 	c->set = true;
 
 	list_add_tail(&c->u.shared_bus_user.node,
-		&c->parent->u.shared_bus.list);
+		&c->parent->shared_bus_list);
 }
 
 static int tegra_clk_shared_bus_set_rate(struct clk *c, unsigned long rate)
@@ -1716,9 +1763,7 @@ static struct clk tegra_clk_sclk = {
 	.reg	= 0x28,
 	.ops	= &tegra_super_ops,
 	.max_rate = 240000000,
-	.u.shared_bus = {
-		.min_rate = 120000000,
-	},
+	.min_rate = 120000000,
 };
 
 static struct clk tegra_clk_virtual_cpu = {
@@ -1843,6 +1888,18 @@ static struct clk_mux_sel mux_clk_32k[] = {
 	{ 0, 0},
 };
 
+static struct clk tegra_clk_emc = {
+	.name = "emc",
+	.ops = &tegra_emc_clk_ops,
+	.reg = 0x19c,
+	.max_rate = 800000000,
+	.inputs = mux_pllm_pllc_pllp_clkm,
+	.flags = MUX | DIV_U71 | PERIPH_EMC_ENB,
+	.u.periph = {
+		.clk_num = 57,
+	},
+};
+
 #define PERIPH_CLK(_name, _dev, _con, _clk_num, _reg, _max, _inputs, _flags) \
 	{						\
 		.name      = _name,			\
@@ -1931,13 +1988,17 @@ struct clk tegra_list_clks[] = {
 	PERIPH_CLK("usbd",	"fsl-tegra-udc",	NULL,	22,	0,	480000000, mux_clk_m,			0), /* requires min voltage */
 	PERIPH_CLK("usb2",	"tegra-ehci.1",		NULL,	58,	0,	480000000, mux_clk_m,			0), /* requires min voltage */
 	PERIPH_CLK("usb3",	"tegra-ehci.2",		NULL,	59,	0,	480000000, mux_clk_m,			0), /* requires min voltage */
-	PERIPH_CLK("emc",	"emc",			NULL,	57,	0x19c,	800000000, mux_pllm_pllc_pllp_clkm,	MUX | DIV_U71 | PERIPH_EMC_ENB),
 	PERIPH_CLK("dsi",	"dsi",			NULL,	48,	0,	500000000, mux_plld,			0), /* scales with voltage */
 	PERIPH_CLK("csi",	"tegra_camera",		"csi",	52,	0,	72000000,  mux_pllp_out3,		0),
 	PERIPH_CLK("isp",	"tegra_camera",		"isp",	23,	0,	150000000, mux_clk_m,			0), /* same frequency as VI */
 	PERIPH_CLK("csus",	"tegra_camera",		"csus",	92,	0,	150000000, mux_clk_m,			PERIPH_NO_RESET),
 
 	SHARED_CLK("avp.sclk",	"tegra-avp",		"sclk",	&tegra_clk_sclk),
+	SHARED_CLK("avp.emc",	"tegra-avp",		"emc",	&tegra_clk_emc),
+	SHARED_CLK("cpu.emc",	"cpu",			"emc",	&tegra_clk_emc),
+	SHARED_CLK("disp1.emc",	"tegradc.0",		"emc",	&tegra_clk_emc),
+	SHARED_CLK("disp2.emc",	"tegradc.1",		"emc",	&tegra_clk_emc),
+	SHARED_CLK("hdmi.emc",	"hdmi",			"emc",	&tegra_clk_emc),
 };
 
 #define CLK_DUPLICATE(_name, _dev, _con)		\
@@ -2012,11 +2073,13 @@ struct clk *tegra_ptr_clks[] = {
 	&tegra_clk_virtual_cpu,
 	&tegra_clk_blink,
 	&tegra_clk_cop,
+	&tegra_clk_emc,
 };
 
 static void tegra2_init_one_clock(struct clk *c)
 {
 	clk_init(c);
+	INIT_LIST_HEAD(&c->shared_bus_list);
 	if (!c->lookup.dev_id && !c->lookup.con_id)
 		c->lookup.con_id = c->name;
 	c->lookup.clk = c;
diff --git a/arch/arm/mach-tegra/tegra2_dvfs.c b/arch/arm/mach-tegra/tegra2_dvfs.c
index 265a7b538f7f..b58a7d2ef92d 100644
--- a/arch/arm/mach-tegra/tegra2_dvfs.c
+++ b/arch/arm/mach-tegra/tegra2_dvfs.c
@@ -20,88 +20,137 @@
 #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"
 
-#define CORE_REGULATOR "vdd_core"
-#define CPU_REGULATOR "vdd_cpu"
+#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};
-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,
+};
+
+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,			\
-		.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 */
+	CORE_DVFS("emc",     1, KHZ, 57000,  333000, 333000, 666000, 666000),
 
 #if 0
 	/*
@@ -110,22 +159,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,54 +182,99 @@ 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),
 };
 
+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 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,19 +284,15 @@ 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",
 				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);
 }
diff --git a/arch/arm/mach-tegra/tegra2_emc.c b/arch/arm/mach-tegra/tegra2_emc.c
new file mode 100644
index 000000000000..bd4fa27b2086
--- /dev/null
+++ b/arch/arm/mach-tegra/tegra2_emc.c
@@ -0,0 +1,172 @@
+/*
+ * 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/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+
+#include <mach/iomap.h>
+
+#include "tegra2_emc.h"
+
+#ifdef CONFIG_TEGRA_EMC_SCALING_ENABLE
+static bool emc_enable = true;
+#else
+static bool emc_enable;
+#endif
+module_param(emc_enable, bool, 0644);
+
+static void __iomem *emc = IO_ADDRESS(TEGRA_EMC_BASE);
+static const struct tegra_emc_table *tegra_emc_table;
+static int tegra_emc_table_size;
+
+static inline void emc_writel(u32 val, unsigned long addr)
+{
+	writel(val, emc + addr);
+}
+
+static inline u32 emc_readl(unsigned long addr)
+{
+	return readl(emc + addr);
+}
+
+static const unsigned long emc_reg_addr[TEGRA_EMC_NUM_REGS] = {
+	0x2c,	/* RC */
+	0x30,	/* RFC */
+	0x34,	/* RAS */
+	0x38,	/* RP */
+	0x3c,	/* R2W */
+	0x40,	/* W2R */
+	0x44,	/* R2P */
+	0x48,	/* W2P */
+	0x4c,	/* RD_RCD */
+	0x50,	/* WR_RCD */
+	0x54,	/* RRD */
+	0x58,	/* REXT */
+	0x5c,	/* WDV */
+	0x60,	/* QUSE */
+	0x64,	/* QRST */
+	0x68,	/* QSAFE */
+	0x6c,	/* RDV */
+	0x70,	/* REFRESH */
+	0x74,	/* BURST_REFRESH_NUM */
+	0x78,	/* PDEX2WR */
+	0x7c,	/* PDEX2RD */
+	0x80,	/* PCHG2PDEN */
+	0x84,	/* ACT2PDEN */
+	0x88,	/* AR2PDEN */
+	0x8c,	/* RW2PDEN */
+	0x90,	/* TXSR */
+	0x94,	/* TCKE */
+	0x98,	/* TFAW */
+	0x9c,	/* TRPAB */
+	0xa0,	/* TCLKSTABLE */
+	0xa4,	/* TCLKSTOP */
+	0xa8,	/* TREFBW */
+	0xac,	/* QUSE_EXTRA */
+	0x114,	/* FBIO_CFG6 */
+	0xb0,	/* ODT_WRITE */
+	0xb4,	/* ODT_READ */
+	0x104,	/* FBIO_CFG5 */
+	0x2bc,	/* CFG_DIG_DLL */
+	0x2c0,	/* DLL_XFORM_DQS */
+	0x2c4,	/* DLL_XFORM_QUSE */
+	0x2e0,	/* ZCAL_REF_CNT */
+	0x2e4,	/* ZCAL_WAIT_CNT */
+	0x2a8,	/* AUTO_CAL_INTERVAL */
+	0x2d0,	/* CFG_CLKTRIM_0 */
+	0x2d4,	/* CFG_CLKTRIM_1 */
+	0x2d8,	/* CFG_CLKTRIM_2 */
+};
+
+/* Select the closest EMC rate that is higher than the requested rate */
+long tegra_emc_round_rate(unsigned long rate)
+{
+	int i;
+	int best = -1;
+	unsigned long distance = ULONG_MAX;
+
+	if (!tegra_emc_table)
+		return -EINVAL;
+
+	if (!emc_enable)
+		return -EINVAL;
+
+	pr_debug("%s: %lu\n", __func__, rate);
+
+	/* The EMC clock rate is twice the bus rate, and the bus rate is
+	 * measured in kHz */
+	rate = rate / 2 / 1000;
+
+	for (i = 0; i < tegra_emc_table_size; i++) {
+		if (tegra_emc_table[i].rate >= rate &&
+		    (tegra_emc_table[i].rate - rate) < distance) {
+			distance = tegra_emc_table[i].rate - rate;
+			best = i;
+		}
+	}
+
+	if (best < 0)
+		return -EINVAL;
+
+	pr_debug("%s: using %lu\n", __func__, tegra_emc_table[best].rate);
+
+	return tegra_emc_table[best].rate * 2 * 1000;
+}
+
+/* The EMC registers have shadow registers.  When the EMC clock is updated
+ * in the clock controller, the shadow registers are copied to the active
+ * registers, allowing glitchless memory bus frequency changes.
+ * This function updates the shadow registers for a new clock frequency,
+ * and relies on the clock lock on the emc clock to avoid races between
+ * multiple frequency changes */
+int tegra_emc_set_rate(unsigned long rate)
+{
+	int i;
+	int j;
+
+	if (!tegra_emc_table)
+		return -EINVAL;
+
+	/* The EMC clock rate is twice the bus rate, and the bus rate is
+	 * measured in kHz */
+	rate = rate / 2 / 1000;
+
+	for (i = 0; i < tegra_emc_table_size; i++)
+		if (tegra_emc_table[i].rate == rate)
+			break;
+
+	if (i >= tegra_emc_table_size)
+		return -EINVAL;
+
+	pr_debug("%s: setting to %lu\n", __func__, rate);
+
+	for (j = 0; j < TEGRA_EMC_NUM_REGS; j++)
+		emc_writel(tegra_emc_table[i].regs[j], emc_reg_addr[j]);
+
+	emc_readl(tegra_emc_table[i].regs[TEGRA_EMC_NUM_REGS - 1]);
+
+	return 0;
+}
+
+void tegra_init_emc(const struct tegra_emc_table *table, int table_size)
+{
+	tegra_emc_table = table;
+	tegra_emc_table_size = table_size;
+}
diff --git a/arch/arm/mach-tegra/tegra2_emc.h b/arch/arm/mach-tegra/tegra2_emc.h
new file mode 100644
index 000000000000..3515e57fd0d9
--- /dev/null
+++ b/arch/arm/mach-tegra/tegra2_emc.h
@@ -0,0 +1,27 @@
+/*
+ * 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.
+ *
+ */
+
+#define TEGRA_EMC_NUM_REGS 46
+
+struct tegra_emc_table {
+	unsigned long rate;
+	u32 regs[TEGRA_EMC_NUM_REGS];
+};
+
+int tegra_emc_set_rate(unsigned long rate);
+long tegra_emc_round_rate(unsigned long rate);
+void tegra_init_emc(const struct tegra_emc_table *table, int table_size);
diff --git a/arch/arm/mach-tegra/tegra_i2s_audio.c b/arch/arm/mach-tegra/tegra_i2s_audio.c
index f3ec7b6fb5f1..f3cd2c42278e 100644
--- a/arch/arm/mach-tegra/tegra_i2s_audio.c
+++ b/arch/arm/mach-tegra/tegra_i2s_audio.c
@@ -695,12 +695,17 @@ static void request_stop_nosync(struct audio_stream *as)
 	pr_debug("%s\n", __func__);
 	if (!as->stop) {
 		as->stop = true;
-		wait_till_stopped(as);
+		if (pending_buffer_requests(as))
+			wait_till_stopped(as);
 		for (i = 0; i < as->num_bufs; i++) {
 			init_completion(&as->comp[i]);
 			complete(&as->comp[i]);
 		}
 	}
+	if (!tegra_dma_is_empty(as->dma_chan))
+		pr_err("%s: DMA not empty!\n", __func__);
+	/* Stop the DMA then dequeue anything that's in progress. */
+	tegra_dma_cancel(as->dma_chan);
 	as->active = false; /* applies to recording only */
 	pr_debug("%s: done\n", __func__);
 }
@@ -826,13 +831,9 @@ static void dma_tx_complete_callback(struct tegra_dma_req *req)
 
 	complete(&aos->comp[req_num]);
 
-	if (stop_playback_if_necessary(aos)) {
-		pr_debug("%s: done (stopped)\n", __func__);
-		if (!completion_done(&aos->stop_completion)) {
-			pr_debug("%s: signalling stop completion\n", __func__);
-			complete(&aos->stop_completion);
-		}
-		return;
+	if (!pending_buffer_requests(aos)) {
+		pr_debug("%s: Playback underflow\n", __func__);
+		complete(&aos->stop_completion);
 	}
 }
 
@@ -851,6 +852,9 @@ static void dma_rx_complete_callback(struct tegra_dma_req *req)
 
 	complete(&ais->comp[req_num]);
 
+	if (!pending_buffer_requests(ais))
+		pr_debug("%s: Capture overflow\n", __func__);
+
 	spin_unlock_irqrestore(&ais->dma_req_lock, flags);
 }
 
@@ -1088,6 +1092,8 @@ static long tegra_audio_out_ioctl(struct file *file,
 			request_stop_nosync(aos);
 			pr_debug("%s: flushed\n", __func__);
 		}
+		if (stop_playback_if_necessary(aos))
+			pr_debug("%s: done (stopped)\n", __func__);
 		aos->stop = false;
 		break;
 	case TEGRA_AUDIO_OUT_SET_NUM_BUFS: {
@@ -1111,6 +1117,7 @@ static long tegra_audio_out_ioctl(struct file *file,
 		if (rc < 0)
 			break;
 		aos->num_bufs = num;
+		sound_ops->setup(ads);
 	}
 		break;
 	case TEGRA_AUDIO_OUT_GET_NUM_BUFS:
@@ -1265,10 +1272,11 @@ static long tegra_audio_in_ioctl(struct file *file,
 		if (rc < 0)
 			break;
 		ais->num_bufs = num;
+		sound_ops->setup(ads);
 	}
 		break;
 	case TEGRA_AUDIO_IN_GET_NUM_BUFS:
-		if (copy_from_user((void __user *)arg,
+		if (copy_to_user((void __user *)arg,
 				&ais->num_bufs, sizeof(ais->num_bufs)))
 			rc = -EFAULT;
 		break;
@@ -1404,6 +1412,8 @@ static int tegra_audio_out_release(struct inode *inode, struct file *file)
 	mutex_lock(&ads->out.lock);
 	ads->out.opened = 0;
 	request_stop_nosync(&ads->out);
+	if (stop_playback_if_necessary(&ads->out))
+		pr_debug("%s: done (stopped)\n", __func__);
 	allow_suspend(&ads->out);
 	mutex_unlock(&ads->out.lock);
 	pr_debug("%s: done\n", __func__);
diff --git a/arch/arm/mach-tegra/tegra_spdif_audio.c b/arch/arm/mach-tegra/tegra_spdif_audio.c
index 0848b1550dfe..6613d3d5edeb 100644
--- a/arch/arm/mach-tegra/tegra_spdif_audio.c
+++ b/arch/arm/mach-tegra/tegra_spdif_audio.c
@@ -460,12 +460,17 @@ static void request_stop_nosync(struct audio_stream *as)
 	pr_debug("%s\n", __func__);
 	if (!as->stop) {
 		as->stop = true;
-		wait_till_stopped(as);
+		if (pending_buffer_requests(as))
+			wait_till_stopped(as);
 		for (i = 0; i < as->num_bufs; i++) {
 			init_completion(&as->comp[i]);
 			complete(&as->comp[i]);
 		}
 	}
+	if (!tegra_dma_is_empty(as->dma_chan))
+		pr_err("%s: DMA not empty!\n", __func__);
+	/* Stop the DMA then dequeue anything that's in progress. */
+	tegra_dma_cancel(as->dma_chan);
 	as->active = false; /* applies to recording only */
 	pr_debug("%s: done\n", __func__);
 }
@@ -543,13 +548,9 @@ static void dma_tx_complete_callback(struct tegra_dma_req *req)
 
 	complete(&aos->comp[req_num]);
 
-	if (stop_playback_if_necessary(aos)) {
-		pr_debug("%s: done (stopped)\n", __func__);
-		if (!completion_done(&aos->stop_completion)) {
-			pr_debug("%s: signalling stop completion\n", __func__);
-			complete(&aos->stop_completion);
-		}
-		return;
+	if (!pending_buffer_requests(aos)) {
+		pr_debug("%s: Playback underflow", __func__);
+		complete(&aos->stop_completion);
 	}
 }
 
@@ -729,6 +730,8 @@ static long tegra_spdif_out_ioctl(struct file *file,
 			request_stop_nosync(aos);
 			pr_debug("%s: flushed\n", __func__);
 		}
+		if (stop_playback_if_necessary(aos))
+			pr_debug("%s: done (stopped)\n", __func__);
 		aos->stop = false;
 		break;
 	case TEGRA_AUDIO_OUT_SET_NUM_BUFS: {
@@ -752,6 +755,7 @@ static long tegra_spdif_out_ioctl(struct file *file,
 		if (rc < 0)
 			break;
 		aos->num_bufs = num;
+		sound_ops->setup(ads);
 	}
 		break;
 	case TEGRA_AUDIO_OUT_GET_NUM_BUFS:
@@ -806,6 +810,8 @@ static int tegra_spdif_out_release(struct inode *inode, struct file *file)
 	mutex_lock(&ads->out.lock);
 	ads->out.opened = 0;
 	request_stop_nosync(&ads->out);
+	if (stop_playback_if_necessary(&ads->out))
+		pr_debug("%s: done (stopped)\n", __func__);
 	allow_suspend(&ads->out);
 	mutex_unlock(&ads->out.lock);
 	pr_debug("%s: done\n", __func__);