1 files changed, 295 insertions, 134 deletions

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;
 }