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)