1 files changed, 244 insertions, 23 deletions

diff --git a/arch/arm/mach-tegra/clock.c b/arch/arm/mach-tegra/clock.c
index 03ad578349b9..ae19f95585be 100644
--- a/arch/arm/mach-tegra/clock.c
+++ b/arch/arm/mach-tegra/clock.c
@@ -24,13 +24,80 @@
 #include <linux/debugfs.h>
 #include <linux/slab.h>
 #include <linux/seq_file.h>
+#include <linux/regulator/consumer.h>
 #include <asm/clkdev.h>
 
 #include "clock.h"
+#include "board.h"
+#include "fuse.h"
 
 static LIST_HEAD(clocks);
 
 static DEFINE_SPINLOCK(clock_lock);
+static DEFINE_MUTEX(dvfs_lock);
+
+static int clk_is_dvfs(struct clk *c)
+{
+	return (c->dvfs != NULL);
+};
+
+static int dvfs_set_rate(struct dvfs *d, unsigned long rate)
+{
+	struct dvfs_table *t;
+
+	if (d->table == NULL)
+		return -ENODEV;
+
+	for (t = d->table; t->rate != 0; t++) {
+		if (rate <= t->rate) {
+			if (!d->reg)
+				return 0;
+
+			return regulator_set_voltage(d->reg,
+				t->millivolts * 1000,
+				d->max_millivolts * 1000);
+		}
+	}
+
+	return -EINVAL;
+}
+
+static void dvfs_init(struct clk *c)
+{
+	int process_id;
+	int i;
+	struct dvfs_table *table;
+
+	process_id = c->dvfs->cpu ? tegra_core_process_id() :
+		tegra_cpu_process_id();
+
+	for (i = 0; i < c->dvfs->process_id_table_length; i++)
+		if (process_id == c->dvfs->process_id_table[i].process_id)
+			c->dvfs->table = c->dvfs->process_id_table[i].table;
+
+	if (c->dvfs->table == NULL) {
+		pr_err("Failed to find dvfs table for clock %s process %d\n",
+			c->name, process_id);
+		return;
+	}
+
+	c->dvfs->max_millivolts = 0;
+	for (table = c->dvfs->table; table->rate != 0; table++)
+		if (c->dvfs->max_millivolts < table->millivolts)
+			c->dvfs->max_millivolts = table->millivolts;
+
+	c->dvfs->reg = regulator_get(NULL, c->dvfs->reg_id);
+
+	if (IS_ERR(c->dvfs->reg)) {
+		pr_err("Failed to get regulator %s for clock %s\n",
+			c->dvfs->reg_id, c->name);
+		c->dvfs->reg = NULL;
+		return;
+	}
+
+	if (c->refcnt > 0)
+		dvfs_set_rate(c->dvfs, c->rate);
+}
 
 struct clk *tegra_get_clock_by_name(const char *name)
 {
@@ -48,14 +115,31 @@ struct clk *tegra_get_clock_by_name(const char *name)
 	return ret;
 }
 
+static void clk_recalculate_rate(struct clk *c)
+{
+	u64 rate;
+
+	if (!c->parent)
+		return;
+
+	rate = c->parent->rate;
+
+	if (c->mul != 0 && c->div != 0) {
+		rate = rate * c->mul;
+		do_div(rate, c->div);
+	}
+
+	if (rate > c->max_rate)
+		pr_warn("clocks: Set clock %s to rate %llu, max is %lu\n",
+			c->name, rate, c->max_rate);
+
+	c->rate = rate;
+}
+
 int clk_reparent(struct clk *c, struct clk *parent)
 {
 	pr_debug("%s: %s\n", __func__, c->name);
-	if (c->refcnt && c->parent)
-		clk_disable_locked(c->parent);
 	c->parent = parent;
-	if (c->refcnt && c->parent)
-		clk_enable_locked(c->parent);
 	list_del(&c->sibling);
 	list_add_tail(&c->sibling, &parent->children);
 	return 0;
@@ -67,8 +151,7 @@ static void propagate_rate(struct clk *c)
 	pr_debug("%s: %s\n", __func__, c->name);
 	list_for_each_entry(clkp, &c->children, sibling) {
 		pr_debug("   %s\n", clkp->name);
-		if (clkp->ops->recalculate_rate)
-			clkp->ops->recalculate_rate(clkp);
+		clk_recalculate_rate(clkp);
 		propagate_rate(clkp);
 	}
 }
@@ -77,6 +160,8 @@ void clk_init(struct clk *c)
 {
 	unsigned long flags;
 
+	pr_debug("%s: %s\n", __func__, c->name);
+
 	spin_lock_irqsave(&clock_lock, flags);
 
 	INIT_LIST_HEAD(&c->children);
@@ -85,6 +170,8 @@ void clk_init(struct clk *c)
 	if (c->ops && c->ops->init)
 		c->ops->init(c);
 
+	clk_recalculate_rate(c);
+
 	list_add(&c->node, &clocks);
 
 	if (c->parent)
@@ -122,13 +209,38 @@ int clk_enable_locked(struct clk *c)
 	return 0;
 }
 
+int clk_enable_cansleep(struct clk *c)
+{
+	int ret;
+	unsigned long flags;
+
+	mutex_lock(&dvfs_lock);
+
+	if (clk_is_dvfs(c) && c->refcnt > 0)
+		dvfs_set_rate(c->dvfs, c->rate);
+
+	spin_lock_irqsave(&clock_lock, flags);
+	ret = clk_enable_locked(c);
+	spin_unlock_irqrestore(&clock_lock, flags);
+
+	mutex_unlock(&dvfs_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(clk_enable_cansleep);
+
 int clk_enable(struct clk *c)
 {
 	int ret;
 	unsigned long flags;
+
+	if (clk_is_dvfs(c))
+		BUG();
+
 	spin_lock_irqsave(&clock_lock, flags);
 	ret = clk_enable_locked(c);
 	spin_unlock_irqrestore(&clock_lock, flags);
+
 	return ret;
 }
 EXPORT_SYMBOL(clk_enable);
@@ -152,9 +264,30 @@ void clk_disable_locked(struct clk *c)
 	c->refcnt--;
 }
 
+void clk_disable_cansleep(struct clk *c)
+{
+	unsigned long flags;
+
+	mutex_lock(&dvfs_lock);
+
+	spin_lock_irqsave(&clock_lock, flags);
+	clk_disable_locked(c);
+	spin_unlock_irqrestore(&clock_lock, flags);
+
+	if (clk_is_dvfs(c) && c->refcnt == 0)
+		dvfs_set_rate(c->dvfs, c->rate);
+
+	mutex_unlock(&dvfs_lock);
+}
+EXPORT_SYMBOL(clk_disable_cansleep);
+
 void clk_disable(struct clk *c)
 {
 	unsigned long flags;
+
+	if (clk_is_dvfs(c))
+		BUG();
+
 	spin_lock_irqsave(&clock_lock, flags);
 	clk_disable_locked(c);
 	spin_unlock_irqrestore(&clock_lock, flags);
@@ -175,6 +308,8 @@ int clk_set_parent_locked(struct clk *c, struct clk *parent)
 	if (ret)
 		return ret;
 
+	clk_recalculate_rate(c);
+
 	propagate_rate(c);
 
 	return 0;
@@ -197,22 +332,69 @@ struct clk *clk_get_parent(struct clk *c)
 }
 EXPORT_SYMBOL(clk_get_parent);
 
-int clk_set_rate(struct clk *c, unsigned long rate)
+int clk_set_rate_locked(struct clk *c, unsigned long rate)
+{
+	int ret;
+
+	if (rate > c->max_rate)
+		rate = c->max_rate;
+
+	if (!c->ops || !c->ops->set_rate)
+		return -ENOSYS;
+
+	ret = c->ops->set_rate(c, rate);
+
+	if (ret)
+		return ret;
+
+	clk_recalculate_rate(c);
+
+	propagate_rate(c);
+
+	return 0;
+}
+
+int clk_set_rate_cansleep(struct clk *c, unsigned long rate)
 {
 	int ret = 0;
 	unsigned long flags;
 
+	pr_debug("%s: %s\n", __func__, c->name);
+
+	mutex_lock(&dvfs_lock);
+
+	if (rate > c->rate)
+		ret = dvfs_set_rate(c->dvfs, rate);
+	if (ret)
+		goto out;
+
 	spin_lock_irqsave(&clock_lock, flags);
+	ret = clk_set_rate_locked(c, rate);
+	spin_unlock_irqrestore(&clock_lock, flags);
 
-	pr_debug("%s: %s\n", __func__, c->name);
+	if (ret)
+		goto out;
 
-	if (c->ops && c->ops->set_rate)
-		ret = c->ops->set_rate(c, rate);
-	else
-		ret = -ENOSYS;
+	ret = dvfs_set_rate(c->dvfs, rate);
 
-	propagate_rate(c);
+out:
+	mutex_unlock(&dvfs_lock);
+	return ret;
+}
+EXPORT_SYMBOL(clk_set_rate_cansleep);
+
+int clk_set_rate(struct clk *c, unsigned long rate)
+{
+	int ret = 0;
+	unsigned long flags;
+
+	pr_debug("%s: %s\n", __func__, c->name);
+
+	if (clk_is_dvfs(c))
+		BUG();
 
+	spin_lock_irqsave(&clock_lock, flags);
+	ret = clk_set_rate_locked(c, rate);
 	spin_unlock_irqrestore(&clock_lock, flags);
 
 	return ret;
@@ -235,6 +417,20 @@ unsigned long clk_get_rate(struct clk *c)
 }
 EXPORT_SYMBOL(clk_get_rate);
 
+long clk_round_rate(struct clk *c, unsigned long rate)
+{
+	pr_debug("%s: %s\n", __func__, c->name);
+
+	if (!c->ops || !c->ops->round_rate)
+		return -ENOSYS;
+
+	if (rate > c->max_rate)
+		rate = c->max_rate;
+
+	return c->ops->round_rate(c, rate);
+}
+EXPORT_SYMBOL(clk_round_rate);
+
 static int tegra_clk_init_one_from_table(struct tegra_clk_init_table *table)
 {
 	struct clk *c;
@@ -308,13 +504,28 @@ void tegra_periph_reset_assert(struct clk *c)
 }
 EXPORT_SYMBOL(tegra_periph_reset_assert);
 
-int __init tegra_init_clock(void)
+void __init tegra_init_clock(void)
 {
 	tegra2_init_clocks();
+}
+
+int __init tegra_init_dvfs(void)
+{
+	struct clk *c, *safe;
+
+	mutex_lock(&dvfs_lock);
+
+	list_for_each_entry_safe(c, safe, &clocks, node)
+		if (c->dvfs)
+			dvfs_init(c);
+
+	mutex_unlock(&dvfs_lock);
 
 	return 0;
 }
 
+late_initcall(tegra_init_dvfs);
+
 #ifdef CONFIG_DEBUG_FS
 static struct dentry *clk_debugfs_root;
 
@@ -324,7 +535,7 @@ static void clock_tree_show_one(struct seq_file *s, struct clk *c, int level)
 	struct clk *child;
 	struct clk *safe;
 	const char *state = "uninit";
-	char div[5] = {0};
+	char div[8] = {0};
 
 	if (c->state == ON)
 		state = "on";
@@ -332,16 +543,26 @@ static void clock_tree_show_one(struct seq_file *s, struct clk *c, int level)
 		state = "off";
 
 	if (c->mul != 0 && c->div != 0) {
-		BUG_ON(c->mul > 2);
-		if (c->mul > c->div)
-			snprintf(div, sizeof(div), "x%d", c->mul / c->div);
-		else
+		if (c->mul > c->div) {
+			int mul = c->mul / c->div;
+			int mul2 = (c->mul * 10 / c->div) % 10;
+			int mul3 = (c->mul * 10) % c->div;
+			if (mul2 == 0 && mul3 == 0)
+				snprintf(div, sizeof(div), "x%d", mul);
+			else if (mul3 == 0)
+				snprintf(div, sizeof(div), "x%d.%d", mul, mul2);
+			else
+				snprintf(div, sizeof(div), "x%d.%d..", mul, mul2);
+		} else {
 			snprintf(div, sizeof(div), "%d%s", c->div / c->mul,
 				(c->div % c->mul) ? ".5" : "");
+		}
 	}
 
-	seq_printf(s, "%*s%-*s %-6s %-3d %-5s %-10lu\n",
-		level * 3 + 1, c->set ? "" : "*",
+	seq_printf(s, "%*s%c%c%-*s %-6s %-3d %-8s %-10lu\n",
+		level * 3 + 1, "",
+		c->rate > c->max_rate ? '!' : ' ',
+		!c->set ? '*' : ' ',
 		30 - level * 3, c->name,
 		state, c->refcnt, div, c->rate);
 	list_for_each_entry_safe(child, safe, &c->children, sibling) {
@@ -353,8 +574,8 @@ static int clock_tree_show(struct seq_file *s, void *data)
 {
 	struct clk *c;
 	unsigned long flags;
-	seq_printf(s, " clock                          state  ref div   rate      \n");
-	seq_printf(s, "-----------------------------------------------------------\n");
+	seq_printf(s, "   clock                          state  ref div      rate\n");
+	seq_printf(s, "--------------------------------------------------------------\n");
 	spin_lock_irqsave(&clock_lock, flags);
 	list_for_each_entry(c, &clocks, node)
 		if (c->parent == NULL)