3 files changed, 56 insertions, 10 deletions

diff --git a/arch/arm/mach-tegra/dvfs.c b/arch/arm/mach-tegra/dvfs.c
index 808570990ae9..2c1e50741f98 100644
--- a/arch/arm/mach-tegra/dvfs.c
+++ b/arch/arm/mach-tegra/dvfs.c
@@ -108,6 +108,7 @@ static int dvfs_rail_set_voltage(struct dvfs_rail *rail, int millivolts)
 	if (rail->disabled)
 		return 0;
 
+	rail->resolving_to = true;
 	steps = DIV_ROUND_UP(abs(millivolts - rail->millivolts), rail->step);
 
 	for (i = 0; i < steps; i++) {
@@ -125,7 +126,7 @@ static int dvfs_rail_set_voltage(struct dvfs_rail *rail, int millivolts)
 		list_for_each_entry(rel, &rail->relationships_to, to_node) {
 			ret = dvfs_rail_update(rel->to);
 			if (ret)
-				return ret;
+				goto out;
 		}
 
 		if (!rail->disabled) {
@@ -137,7 +138,7 @@ static int dvfs_rail_set_voltage(struct dvfs_rail *rail, int millivolts)
 		}
 		if (ret) {
 			pr_err("Failed to set dvfs regulator %s\n", rail->reg_id);
-			return ret;
+			goto out;
 		}
 
 		rail->millivolts = rail->new_millivolts;
@@ -149,16 +150,18 @@ static int dvfs_rail_set_voltage(struct dvfs_rail *rail, int millivolts)
 		list_for_each_entry(rel, &rail->relationships_to, to_node) {
 			ret = dvfs_rail_update(rel->to);
 			if (ret)
-				return ret;
+				goto out;
 		}
 	}
 
 	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;
+		ret = -EINVAL;
 	}
 
+out:
+	rail->resolving_to = false;
 	return ret;
 }
 
@@ -181,6 +184,11 @@ static int dvfs_rail_update(struct dvfs_rail *rail)
 	if (!rail->reg)
 		return 0;
 
+	/* if rail update is entered while resolving circular dependencies,
+	   abort recursion */
+	if (rail->resolving_to)
+		return 0;
+
 	/* Find the maximum voltage requested by any clock */
 	list_for_each_entry(d, &rail->dvfs, reg_node)
 		millivolts = max(d->cur_millivolts, millivolts);
@@ -312,13 +320,14 @@ static bool tegra_dvfs_all_rails_suspended(void)
 	return all_suspended;
 }
 
-static bool tegra_dvfs_from_rails_suspended(struct dvfs_rail *to)
+static bool tegra_dvfs_from_rails_suspended_or_solved(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)
+		if (!rel->from->suspended && !rel->from->disabled &&
+			!rel->solved_at_nominal)
 			all_suspended = false;
 
 	return all_suspended;
@@ -331,7 +340,7 @@ static int tegra_dvfs_suspend_one(void)
 
 	list_for_each_entry(rail, &dvfs_rail_list, node) {
 		if (!rail->suspended && !rail->disabled &&
-		    tegra_dvfs_from_rails_suspended(rail)) {
+		    tegra_dvfs_from_rails_suspended_or_solved(rail)) {
 			ret = dvfs_rail_set_voltage(rail,
 				rail->nominal_millivolts);
 			if (ret)
diff --git a/arch/arm/mach-tegra/dvfs.h b/arch/arm/mach-tegra/dvfs.h
index f33cb2c6408c..6de35a525411 100644
--- a/arch/arm/mach-tegra/dvfs.h
+++ b/arch/arm/mach-tegra/dvfs.h
@@ -40,6 +40,7 @@ struct dvfs_relationship {
 
 	struct list_head to_node; /* node in relationship_to list */
 	struct list_head from_node; /* node in relationship_from list */
+	bool solved_at_nominal;
 };
 
 struct dvfs_rail {
@@ -50,6 +51,7 @@ struct dvfs_rail {
 	int step;
 	bool disabled;
 	bool updating;
+	bool resolving_to;
 
 	struct list_head node;  /* node in dvfs_rail_list */
 	struct list_head dvfs;  /* list head of attached dvfs clocks */
diff --git a/arch/arm/mach-tegra/tegra3_dvfs.c b/arch/arm/mach-tegra/tegra3_dvfs.c
index 24685fa271c8..3e04bd5d3059 100644
--- a/arch/arm/mach-tegra/tegra3_dvfs.c
+++ b/arch/arm/mach-tegra/tegra3_dvfs.c
@@ -35,11 +35,14 @@ static const int core_millivolts[MAX_DVFS_FREQS] =
 #define KHZ 1000
 #define MHZ 1000000
 
+#define VDD_CPU_BELOW_VDD_CORE_MAX	300
+
 static struct dvfs_rail tegra3_dvfs_rail_vdd_cpu = {
 	.reg_id = "vdd_cpu",
 	.max_millivolts = 1125,
 	.min_millivolts = 800,
 	.nominal_millivolts = 1000,
+	.step = VDD_CPU_BELOW_VDD_CORE_MAX,
 };
 
 static struct dvfs_rail tegra3_dvfs_rail_vdd_core = {
@@ -47,7 +50,7 @@ static struct dvfs_rail tegra3_dvfs_rail_vdd_core = {
 	.max_millivolts = 1300,
 	.min_millivolts = 950,
 	.nominal_millivolts = 1200,
-	.step = 100, /* FIXME: step vdd_core by 100 mV - maybe not needed */
+	.step = VDD_CPU_BELOW_VDD_CORE_MAX,
 	.disabled = true, /* FIXME: replace with sysfs control */
 };
 
@@ -56,6 +59,38 @@ static struct dvfs_rail *tegra3_dvfs_rails[] = {
 	&tegra3_dvfs_rail_vdd_core,
 };
 
+
+/* vdd_core must not be lower than vdd_cpu */
+static int tegra3_dvfs_rel_vdd_cpu_vdd_core(struct dvfs_rail *vdd_cpu,
+	struct dvfs_rail *vdd_core)
+{
+	int core_floor = max(vdd_cpu->new_millivolts, vdd_cpu->millivolts);
+	return max(vdd_core->new_millivolts, core_floor);
+}
+
+/* vdd_cpu must be within VDD_CPU_BELOW_VDD_CORE_MAX below vdd_core */
+static int tegra3_dvfs_rel_vdd_core_vdd_cpu(struct dvfs_rail *vdd_core,
+	struct dvfs_rail *vdd_cpu)
+{
+	int cpu_floor = max(vdd_core->new_millivolts, vdd_core->millivolts) -
+		VDD_CPU_BELOW_VDD_CORE_MAX;
+	return max(vdd_cpu->new_millivolts, cpu_floor);
+}
+
+static struct dvfs_relationship tegra3_dvfs_relationships[] = {
+	{
+		.from = &tegra3_dvfs_rail_vdd_cpu,
+		.to = &tegra3_dvfs_rail_vdd_core,
+		.solve = tegra3_dvfs_rel_vdd_cpu_vdd_core,
+		.solved_at_nominal = true,
+	},
+	{
+		.from = &tegra3_dvfs_rail_vdd_core,
+		.to = &tegra3_dvfs_rail_vdd_cpu,
+		.solve = tegra3_dvfs_rel_vdd_core_vdd_cpu,
+	},
+};
+
 #define CPU_DVFS(_clk_name, _speedo_id, _process_id, _mult, _freqs...)	\
 	{								\
 		.clk_name	= _clk_name,				\
@@ -220,8 +255,8 @@ void __init tegra_soc_init_dvfs(void)
 #endif
 
 	tegra_dvfs_init_rails(tegra3_dvfs_rails, ARRAY_SIZE(tegra3_dvfs_rails));
-
-	/* FIXME: add [CPU/CORE/AON] relationships here */
+	tegra_dvfs_add_relationships(tegra3_dvfs_relationships,
+		ARRAY_SIZE(tegra3_dvfs_relationships));
 
 	init_dvfs_from_table(cpu_dvfs_table, ARRAY_SIZE(cpu_dvfs_table),
 			     speedo_id, cpu_process_id);