ARM: tegra: dvfs: Move common core voltage capping code
Alex Frid [Thu, 24 Jan 2013 01:25:11 +0000 (17:25 -0800)]
Moved common for Tegra3 and Tegra11 core voltage capping code to
a separate file.

Change-Id: I69c365abdaba80dae64d07c3c3c1c9f8d42cff19
Signed-off-by: Alex Frid <afrid@nvidia.com>
Reviewed-on: http://git-master/r/193602
Reviewed-by: Automatic_Commit_Validation_User
Reviewed-by: Diwakar Tundlam <dtundlam@nvidia.com>
Reviewed-by: Seshendra Gadagottu <sgadagottu@nvidia.com>
Reviewed-by: Kaz Fukuoka <kfukuoka@nvidia.com>

arch/arm/mach-tegra/Makefile
arch/arm/mach-tegra/dvfs.h
arch/arm/mach-tegra/tegra11_dvfs.c
arch/arm/mach-tegra/tegra3_dvfs.c
arch/arm/mach-tegra/tegra_core_volt_cap.c [new file with mode: 0644]

index 011ba18..aa12a15 100644 (file)
@@ -31,6 +31,7 @@ obj-y                                   += pm-t3.o
 obj-y                                   += sleep-t3.o
 obj-$(CONFIG_DEBUG_FS)                  += clocks_stats.o
 obj-y                                   += timer-t3.o
+obj-y                                   += tegra_core_volt_cap.o
 ifeq ($(CONFIG_ARCH_TEGRA_3x_SOC),y)
 obj-y                                   += wakeups-t3.o
 else
index ba4c8f8..468db67 100644 (file)
@@ -167,6 +167,17 @@ struct dvfs_data {
        unsigned int num_voltages;
 };
 
+struct core_dvfs_cap_table {
+       const char *cap_name;
+       struct clk *cap_clk;
+       unsigned long freqs[MAX_DVFS_FREQS];
+};
+
+struct core_cap {
+       int refcnt;
+       int level;
+};
+
 #ifdef CONFIG_OF
 typedef int (*of_tegra_dvfs_init_cb_t)(struct device_node *);
 int of_tegra_dvfs_init(const struct of_device_id *matches);
@@ -220,6 +231,10 @@ static inline void tegra_dvfs_age_cpu(int cur_linear_age)
 { return; }
 #endif
 
+int tegra_init_core_cap(struct core_dvfs_cap_table *table, int table_size,
+       const int *millivolts, int millivolts_num, struct kobject *cap_kobj);
+
+
 static inline bool tegra_dvfs_rail_is_dfll_mode(struct dvfs_rail *rail)
 {
        return rail->dfll_mode;
index 682196e..6cff1e2 100644 (file)
@@ -770,27 +770,10 @@ int tegra_dvfs_rail_post_enable(struct dvfs_rail *rail)
        return 0;
 }
 
-/*
- * sysfs and dvfs interfaces to cap tegra core domains frequencies
- */
-static DEFINE_MUTEX(core_cap_lock);
-
-struct core_cap {
-       int refcnt;
-       int level;
-};
-static struct core_cap core_buses_cap;
-static struct core_cap kdvfs_core_cap;
-static struct core_cap user_core_cap;
-
+/* Core cap object and table */
 static struct kobject *cap_kobj;
 
-/* Arranged in order required for enabling/lowering the cap */
-static struct {
-       const char *cap_name;
-       struct clk *cap_clk;
-       unsigned long freqs[MAX_DVFS_FREQS];
-} core_cap_table[] = {
+static struct core_dvfs_cap_table tegra11_core_cap_table[] = {
 #ifdef CONFIG_TEGRA_DUAL_CBUS
        { .cap_name = "cap.c2bus" },
        { .cap_name = "cap.c3bus" },
@@ -801,207 +784,9 @@ static struct {
        { .cap_name = "cap.emc" },
 };
 
-
-static void core_cap_level_set(int level)
-{
-       int i, j;
-
-       for (j = 0; j < ARRAY_SIZE(core_millivolts); j++) {
-               int v = core_millivolts[j];
-               if ((v == 0) || (level < v))
-                       break;
-       }
-       j = (j == 0) ? 0 : j - 1;
-       level = core_millivolts[j];
-
-       if (level < core_buses_cap.level) {
-               for (i = 0; i < ARRAY_SIZE(core_cap_table); i++)
-                       if (core_cap_table[i].cap_clk)
-                               clk_set_rate(core_cap_table[i].cap_clk,
-                                            core_cap_table[i].freqs[j]);
-       } else if (level > core_buses_cap.level) {
-               for (i = ARRAY_SIZE(core_cap_table) - 1; i >= 0; i--)
-                       if (core_cap_table[i].cap_clk)
-                               clk_set_rate(core_cap_table[i].cap_clk,
-                                            core_cap_table[i].freqs[j]);
-       }
-       core_buses_cap.level = level;
-}
-
-static void core_cap_update(void)
-{
-       int new_level = tegra11_dvfs_rail_vdd_core.max_millivolts;
-
-       if (kdvfs_core_cap.refcnt)
-               new_level = min(new_level, kdvfs_core_cap.level);
-       if (user_core_cap.refcnt)
-               new_level = min(new_level, user_core_cap.level);
-
-       if (core_buses_cap.level != new_level)
-               core_cap_level_set(new_level);
-}
-
-static void core_cap_enable(bool enable)
-{
-       if (enable)
-               core_buses_cap.refcnt++;
-       else if (core_buses_cap.refcnt)
-               core_buses_cap.refcnt--;
-
-       core_cap_update();
-}
-
-static ssize_t
-core_cap_state_show(struct kobject *kobj, struct kobj_attribute *attr,
-                   char *buf)
-{
-       return sprintf(buf, "%d (%d)\n", core_buses_cap.refcnt ? 1 : 0,
-                       user_core_cap.refcnt ? 1 : 0);
-}
-static ssize_t
-core_cap_state_store(struct kobject *kobj, struct kobj_attribute *attr,
-                    const char *buf, size_t count)
-{
-       int state;
-
-       if (sscanf(buf, "%d", &state) != 1)
-               return -1;
-
-       mutex_lock(&core_cap_lock);
-
-       if (state) {
-               user_core_cap.refcnt++;
-               if (user_core_cap.refcnt == 1)
-                       core_cap_enable(true);
-       } else if (user_core_cap.refcnt) {
-               user_core_cap.refcnt--;
-               if (user_core_cap.refcnt == 0)
-                       core_cap_enable(false);
-       }
-
-       mutex_unlock(&core_cap_lock);
-       return count;
-}
-
-static ssize_t
-core_cap_level_show(struct kobject *kobj, struct kobj_attribute *attr,
-                   char *buf)
-{
-       return sprintf(buf, "%d (%d)\n", core_buses_cap.level,
-                       user_core_cap.level);
-}
-static ssize_t
-core_cap_level_store(struct kobject *kobj, struct kobj_attribute *attr,
-                    const char *buf, size_t count)
-{
-       int level;
-
-       if (sscanf(buf, "%d", &level) != 1)
-               return -1;
-
-       mutex_lock(&core_cap_lock);
-       user_core_cap.level = level;
-       core_cap_update();
-       mutex_unlock(&core_cap_lock);
-       return count;
-}
-
-static struct kobj_attribute cap_state_attribute =
-       __ATTR(core_cap_state, 0644, core_cap_state_show, core_cap_state_store);
-static struct kobj_attribute cap_level_attribute =
-       __ATTR(core_cap_level, 0644, core_cap_level_show, core_cap_level_store);
-
-const struct attribute *cap_attributes[] = {
-       &cap_state_attribute.attr,
-       &cap_level_attribute.attr,
-       NULL,
-};
-
-void tegra_dvfs_core_cap_enable(bool enable)
-{
-       mutex_lock(&core_cap_lock);
-
-       if (enable) {
-               kdvfs_core_cap.refcnt++;
-               if (kdvfs_core_cap.refcnt == 1)
-                       core_cap_enable(true);
-       } else if (kdvfs_core_cap.refcnt) {
-               kdvfs_core_cap.refcnt--;
-               if (kdvfs_core_cap.refcnt == 0)
-                       core_cap_enable(false);
-       }
-       mutex_unlock(&core_cap_lock);
-}
-
-void tegra_dvfs_core_cap_level_set(int level)
-{
-       mutex_lock(&core_cap_lock);
-       kdvfs_core_cap.level = level;
-       core_cap_update();
-       mutex_unlock(&core_cap_lock);
-}
-
-static int __init init_core_cap_one(struct clk *c, unsigned long *freqs)
+static int __init tegra11_dvfs_init_core_cap(void)
 {
-       int i, v, next_v = 0;
-       unsigned long rate, next_rate = 0;
-
-       for (i = 0; i < ARRAY_SIZE(core_millivolts); i++) {
-               v = core_millivolts[i];
-               if (v == 0)
-                       break;
-
-               for (;;) {
-                       rate = next_rate;
-                       next_rate = clk_round_rate(c->parent, rate + 1000);
-                       if (IS_ERR_VALUE(next_rate)) {
-                               pr_debug("tegra11_dvfs: failed to round %s rate %lu\n",
-                                        c->name, rate);
-                               return -EINVAL;
-                       }
-                       if (rate == next_rate)
-                               break;
-
-                       next_v = tegra_dvfs_predict_millivolts(
-                               c->parent, next_rate);
-                       if (IS_ERR_VALUE(next_v)) {
-                               pr_debug("tegra11_dvfs: failed to predict %s mV for rate %lu\n",
-                                        c->name, next_rate);
-                               return -EINVAL;
-                       }
-                       if (next_v > v)
-                               break;
-               }
-
-               if (rate == 0) {
-                       rate = next_rate;
-                       pr_warn("tegra11_dvfs: minimum %s rate %lu requires %d mV\n",
-                               c->name, rate, next_v);
-               }
-               freqs[i] = rate;
-               next_rate = rate;
-       }
-       return 0;
-}
-
-static int __init tegra_dvfs_init_core_cap(void)
-{
-       int i;
-       struct clk *c = NULL;
-
-       core_buses_cap.level = kdvfs_core_cap.level = user_core_cap.level =
-               tegra11_dvfs_rail_vdd_core.max_millivolts;
-
-       for (i = 0; i < ARRAY_SIZE(core_cap_table); i++) {
-               c = tegra_get_clock_by_name(core_cap_table[i].cap_name);
-               if (!c || !c->parent ||
-                   init_core_cap_one(c, core_cap_table[i].freqs)) {
-                       pr_err("tegra11_dvfs: failed to initialize %s table\n",
-                              core_cap_table[i].cap_name);
-                       continue;
-               }
-               core_cap_table[i].cap_clk = c;
-       }
+       int ret;
 
        cap_kobj = kobject_create_and_add("tegra_cap", kernel_kobj);
        if (!cap_kobj) {
@@ -1009,12 +794,18 @@ static int __init tegra_dvfs_init_core_cap(void)
                return 0;
        }
 
-       if (sysfs_create_files(cap_kobj, cap_attributes)) {
-               pr_err("tegra11_dvfs: failed to create sysfs cap interface\n");
+       ret = tegra_init_core_cap(
+               tegra11_core_cap_table, ARRAY_SIZE(tegra11_core_cap_table),
+               core_millivolts, ARRAY_SIZE(core_millivolts), cap_kobj);
+
+       if (ret) {
+               pr_err("tegra11_dvfs: failed to init core cap interface (%d)\n",
+                      ret);
+               kobject_del(cap_kobj);
                return 0;
        }
        pr_info("tegra dvfs: tegra sysfs cap interface is initialized\n");
 
        return 0;
 }
-late_initcall(tegra_dvfs_init_core_cap);
+late_initcall(tegra11_dvfs_init_core_cap);
index bf2ae2f..cb25a17 100644 (file)
@@ -1020,138 +1020,19 @@ int tegra_dvfs_rail_post_enable(struct dvfs_rail *rail)
        return 0;
 }
 
-/*
- * sysfs and dvfs interfaces to cap tegra core domains frequencies
- */
-static DEFINE_MUTEX(core_cap_lock);
-
-struct core_cap {
-       int refcnt;
-       int level;
-};
-static struct core_cap tegra3_core_cap;
-static struct core_cap kdvfs_core_cap;
-static struct core_cap user_core_cap;
-
-static struct core_cap user_cbus_cap;
-
+/* Core cap object and table */
 static struct kobject *cap_kobj;
 
 /* Arranged in order required for enabling/lowering the cap */
-static struct {
-       const char *cap_name;
-       struct clk *cap_clk;
-       unsigned long freqs[MAX_DVFS_FREQS];
-} core_cap_table[] = {
+static struct core_dvfs_cap_table tegra3_core_cap_table[] = {
        { .cap_name = "cap.cbus" },
        { .cap_name = "cap.sclk" },
        { .cap_name = "cap.emc" },
 };
 
-
-static void core_cap_level_set(int level)
-{
-       int i, j;
-
-       for (j = 0; j < core_dvfs_data->num_voltages; j++) {
-               int v = core_dvfs_data->millivolts[j];
-               if ((v == 0) || (level < v))
-                       break;
-       }
-       j = (j == 0) ? 0 : j - 1;
-       level = core_dvfs_data->millivolts[j];
-
-       if (level < tegra3_core_cap.level) {
-               for (i = 0; i < ARRAY_SIZE(core_cap_table); i++)
-                       if (core_cap_table[i].cap_clk)
-                               clk_set_rate(core_cap_table[i].cap_clk,
-                                            core_cap_table[i].freqs[j]);
-       } else if (level > tegra3_core_cap.level) {
-               for (i = ARRAY_SIZE(core_cap_table) - 1; i >= 0; i--)
-                       if (core_cap_table[i].cap_clk)
-                               clk_set_rate(core_cap_table[i].cap_clk,
-                                            core_cap_table[i].freqs[j]);
-       }
-       tegra3_core_cap.level = level;
-}
-
-static void core_cap_update(void)
-{
-       int new_level = tegra3_dvfs_rail_vdd_core.max_millivolts;
-
-       if (kdvfs_core_cap.refcnt)
-               new_level = min(new_level, kdvfs_core_cap.level);
-       if (user_core_cap.refcnt)
-               new_level = min(new_level, user_core_cap.level);
-
-       if (tegra3_core_cap.level != new_level)
-               core_cap_level_set(new_level);
-}
-
-static void core_cap_enable(bool enable)
-{
-       if (enable)
-               tegra3_core_cap.refcnt++;
-       else if (tegra3_core_cap.refcnt)
-               tegra3_core_cap.refcnt--;
-
-       core_cap_update();
-}
-
-static ssize_t
-core_cap_state_show(struct kobject *kobj, struct kobj_attribute *attr,
-                   char *buf)
-{
-       return sprintf(buf, "%d (%d)\n", tegra3_core_cap.refcnt ? 1 : 0,
-                       user_core_cap.refcnt ? 1 : 0);
-}
-static ssize_t
-core_cap_state_store(struct kobject *kobj, struct kobj_attribute *attr,
-                    const char *buf, size_t count)
-{
-       int state;
-
-       if (sscanf(buf, "%d", &state) != 1)
-               return -1;
-
-       mutex_lock(&core_cap_lock);
-
-       if (state) {
-               user_core_cap.refcnt++;
-               if (user_core_cap.refcnt == 1)
-                       core_cap_enable(true);
-       } else if (user_core_cap.refcnt) {
-               user_core_cap.refcnt--;
-               if (user_core_cap.refcnt == 0)
-                       core_cap_enable(false);
-       }
-
-       mutex_unlock(&core_cap_lock);
-       return count;
-}
-
-static ssize_t
-core_cap_level_show(struct kobject *kobj, struct kobj_attribute *attr,
-                   char *buf)
-{
-       return sprintf(buf, "%d (%d)\n", tegra3_core_cap.level,
-                       user_core_cap.level);
-}
-static ssize_t
-core_cap_level_store(struct kobject *kobj, struct kobj_attribute *attr,
-                    const char *buf, size_t count)
-{
-       int level;
-
-       if (sscanf(buf, "%d", &level) != 1)
-               return -1;
-
-       mutex_lock(&core_cap_lock);
-       user_core_cap.level = level;
-       core_cap_update();
-       mutex_unlock(&core_cap_lock);
-       return count;
-}
+/* cbus profiler cap */
+static DEFINE_MUTEX(cbus_cup_lock);
+static struct core_cap user_cbus_cap;
 
 static void cbus_cap_update(void)
 {
@@ -1186,7 +1067,7 @@ cbus_cap_state_store(struct kobject *kobj, struct kobj_attribute *attr,
        if (sscanf(buf, "%d", &state) != 1)
                return -1;
 
-       mutex_lock(&core_cap_lock);
+       mutex_lock(&cbus_cup_lock);
 
        if (state) {
                user_cbus_cap.refcnt++;
@@ -1198,7 +1079,7 @@ cbus_cap_state_store(struct kobject *kobj, struct kobj_attribute *attr,
                        cbus_cap_update();
        }
 
-       mutex_unlock(&core_cap_lock);
+       mutex_unlock(&cbus_cup_lock);
        return count;
 }
 
@@ -1217,128 +1098,54 @@ cbus_cap_level_store(struct kobject *kobj, struct kobj_attribute *attr,
        if (sscanf(buf, "%d", &level) != 1)
                return -1;
 
-       mutex_lock(&core_cap_lock);
+       mutex_lock(&cbus_cup_lock);
        user_cbus_cap.level = level;
        cbus_cap_update();
-       mutex_unlock(&core_cap_lock);
+       mutex_unlock(&cbus_cup_lock);
        return count;
 }
 
-static struct kobj_attribute cap_state_attribute =
-       __ATTR(core_cap_state, 0644, core_cap_state_show, core_cap_state_store);
-static struct kobj_attribute cap_level_attribute =
-       __ATTR(core_cap_level, 0644, core_cap_level_show, core_cap_level_store);
 static struct kobj_attribute cbus_state_attribute =
        __ATTR(cbus_cap_state, 0644, cbus_cap_state_show, cbus_cap_state_store);
 static struct kobj_attribute cbus_level_attribute =
        __ATTR(cbus_cap_level, 0644, cbus_cap_level_show, cbus_cap_level_store);
 
-const struct attribute *cap_attributes[] = {
-       &cap_state_attribute.attr,
-       &cap_level_attribute.attr,
+const struct attribute *cbus_cap_attributes[] = {
        &cbus_state_attribute.attr,
        &cbus_level_attribute.attr,
        NULL,
 };
 
-void tegra_dvfs_core_cap_enable(bool enable)
-{
-       mutex_lock(&core_cap_lock);
-
-       if (enable) {
-               kdvfs_core_cap.refcnt++;
-               if (kdvfs_core_cap.refcnt == 1)
-                       core_cap_enable(true);
-       } else if (kdvfs_core_cap.refcnt) {
-               kdvfs_core_cap.refcnt--;
-               if (kdvfs_core_cap.refcnt == 0)
-                       core_cap_enable(false);
-       }
-       mutex_unlock(&core_cap_lock);
-}
-
-void tegra_dvfs_core_cap_level_set(int level)
-{
-       mutex_lock(&core_cap_lock);
-       kdvfs_core_cap.level = level;
-       core_cap_update();
-       mutex_unlock(&core_cap_lock);
-}
-
-static int __init init_core_cap_one(struct clk *c, unsigned long *freqs)
-{
-       int i, v, next_v = 0;
-       unsigned long rate, next_rate = 0;
-
-       for (i = 0; i < core_dvfs_data->num_voltages; i++) {
-               v = core_dvfs_data->millivolts[i];
-               if (v == 0)
-                       break;
-
-               for (;;) {
-                       rate = next_rate;
-                       next_rate = clk_round_rate(c, rate + 1000);
-                       if (IS_ERR_VALUE(next_rate)) {
-                               pr_debug("tegra3_dvfs: failed to round %s"
-                                          " rate %lu", c->name, rate);
-                               return -EINVAL;
-                       }
-                       if (rate == next_rate)
-                               break;
-
-                       next_v = tegra_dvfs_predict_millivolts(
-                               c->parent, next_rate);
-                       if (IS_ERR_VALUE(next_v)) {
-                               pr_debug("tegra3_dvfs: failed to predict %s mV"
-                                        " for rate %lu", c->name, next_rate);
-                               return -EINVAL;
-                       }
-                       if (next_v > v)
-                               break;
-               }
-
-               if (rate == 0) {
-                       rate = next_rate;
-                       pr_warn("tegra3_dvfs: minimum %s rate %lu requires"
-                               " %d mV", c->name, rate, next_v);
-               }
-               freqs[i] = rate;
-               next_rate = rate;
-       }
-       return 0;
-}
-
-static int __init tegra_dvfs_init_core_cap(void)
+static int __init tegra3_dvfs_init_core_cap(void)
 {
-       int i;
-       struct clk *c = NULL;
-
-       tegra3_core_cap.level = kdvfs_core_cap.level = user_core_cap.level =
-               tegra3_dvfs_rail_vdd_core.max_millivolts;
-
-       for (i = 0; i < ARRAY_SIZE(core_cap_table); i++) {
-               c = tegra_get_clock_by_name(core_cap_table[i].cap_name);
-               if (!c || !c->parent ||
-                   init_core_cap_one(c, core_cap_table[i].freqs)) {
-                       pr_err("tegra3_dvfs: failed to initialize %s frequency"
-                              " table", core_cap_table[i].cap_name);
-                       continue;
-               }
-               core_cap_table[i].cap_clk = c;
-       }
+       int ret;
 
        cap_kobj = kobject_create_and_add("tegra_cap", kernel_kobj);
        if (!cap_kobj) {
-               pr_err("tegra3_dvfs: failed to create sysfs cap object");
+               pr_err("tegra3_dvfs: failed to create sysfs cap object\n");
                return 0;
        }
 
-       if (sysfs_create_files(cap_kobj, cap_attributes)) {
-               pr_err("tegra3_dvfs: failed to create sysfs cap interface");
+       ret = sysfs_create_files(cap_kobj, cbus_cap_attributes);
+       if (ret) {
+               pr_err("tegra3_dvfs: failed to init cbus cap interface (%d)\n",
+                      ret);
+               kobject_del(cap_kobj);
+               return 0;
+       }
+
+       ret = tegra_init_core_cap(
+               tegra3_core_cap_table, ARRAY_SIZE(tegra3_core_cap_table),
+               core_millivolts, ARRAY_SIZE(core_millivolts), cap_kobj);
+
+       if (ret) {
+               pr_err("tegra11_dvfs: failed to init core cap interface (%d)\n",
+                      ret);
+               kobject_del(cap_kobj);
                return 0;
        }
        pr_info("tegra dvfs: tegra sysfs cap interface is initialized\n");
 
        return 0;
 }
-late_initcall(tegra_dvfs_init_core_cap);
+late_initcall(tegra3_dvfs_init_core_cap);
diff --git a/arch/arm/mach-tegra/tegra_core_volt_cap.c b/arch/arm/mach-tegra/tegra_core_volt_cap.c
new file mode 100644 (file)
index 0000000..6cc25aa
--- /dev/null
@@ -0,0 +1,275 @@
+/*
+ * arch/arm/mach-tegra/tegra_core_volt_cap.c
+ *
+ * Copyright (C) 2013 NVIDIA Corporation.
+ *
+ * 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/init.h>
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/kobject.h>
+#include <linux/err.h>
+
+#include "clock.h"
+#include "dvfs.h"
+
+/*
+ * sysfs and kernel interfaces to limit tegra core shared bus frequencies based
+ * on the required core voltage (cap level)
+ *
+ * Cap level is specified in millivolts, and rate limits from the respective
+ * dvfs tables are applied to all bus clocks. Note that cap level affects only
+ * scalable bus frequencies (graphics bus, emc, system clock). Core voltage is
+ * not necessarily set at the cap level, since CPU and/or fixed peripheral
+ * clocks outside the buses may require higher voltage level.
+ */
+static DEFINE_MUTEX(core_cap_lock);
+
+static struct core_cap core_buses_cap;
+static struct core_cap kdvfs_core_cap;
+static struct core_cap user_core_cap;
+
+static struct core_dvfs_cap_table *core_cap_table;
+static int core_cap_table_size;
+
+static const int *cap_millivolts;
+static int cap_millivolts_num;
+
+static int core_nominal_mv;
+
+static void core_cap_level_set(int level)
+{
+       int i, j;
+
+       if (!core_cap_table)
+               return;
+
+       for (j = 0; j < cap_millivolts_num; j++) {
+               int v = cap_millivolts[j];
+               if ((v == 0) || (level < v))
+                       break;
+       }
+       j = (j == 0) ? 0 : j - 1;
+       level = cap_millivolts[j];
+
+       if (level < core_buses_cap.level) {
+               for (i = 0; i < core_cap_table_size; i++)
+                       if (core_cap_table[i].cap_clk)
+                               clk_set_rate(core_cap_table[i].cap_clk,
+                                            core_cap_table[i].freqs[j]);
+       } else if (level > core_buses_cap.level) {
+               for (i = core_cap_table_size - 1; i >= 0; i--)
+                       if (core_cap_table[i].cap_clk)
+                               clk_set_rate(core_cap_table[i].cap_clk,
+                                            core_cap_table[i].freqs[j]);
+       }
+       core_buses_cap.level = level;
+}
+
+static void core_cap_update(void)
+{
+       int new_level = core_nominal_mv;
+
+       if (kdvfs_core_cap.refcnt)
+               new_level = min(new_level, kdvfs_core_cap.level);
+       if (user_core_cap.refcnt)
+               new_level = min(new_level, user_core_cap.level);
+
+       if (core_buses_cap.level != new_level)
+               core_cap_level_set(new_level);
+}
+
+static void core_cap_enable(bool enable)
+{
+       if (enable)
+               core_buses_cap.refcnt++;
+       else if (core_buses_cap.refcnt)
+               core_buses_cap.refcnt--;
+
+       core_cap_update();
+}
+
+static ssize_t
+core_cap_state_show(struct kobject *kobj, struct kobj_attribute *attr,
+                   char *buf)
+{
+       return sprintf(buf, "%d (%d)\n", core_buses_cap.refcnt ? 1 : 0,
+                       user_core_cap.refcnt ? 1 : 0);
+}
+static ssize_t
+core_cap_state_store(struct kobject *kobj, struct kobj_attribute *attr,
+                    const char *buf, size_t count)
+{
+       int state;
+
+       if (sscanf(buf, "%d", &state) != 1)
+               return -1;
+
+       mutex_lock(&core_cap_lock);
+
+       if (state) {
+               user_core_cap.refcnt++;
+               if (user_core_cap.refcnt == 1)
+                       core_cap_enable(true);
+       } else if (user_core_cap.refcnt) {
+               user_core_cap.refcnt--;
+               if (user_core_cap.refcnt == 0)
+                       core_cap_enable(false);
+       }
+
+       mutex_unlock(&core_cap_lock);
+       return count;
+}
+
+static ssize_t
+core_cap_level_show(struct kobject *kobj, struct kobj_attribute *attr,
+                   char *buf)
+{
+       return sprintf(buf, "%d (%d)\n", core_buses_cap.level,
+                       user_core_cap.level);
+}
+static ssize_t
+core_cap_level_store(struct kobject *kobj, struct kobj_attribute *attr,
+                    const char *buf, size_t count)
+{
+       int level;
+
+       if (sscanf(buf, "%d", &level) != 1)
+               return -1;
+
+       mutex_lock(&core_cap_lock);
+       user_core_cap.level = level;
+       core_cap_update();
+       mutex_unlock(&core_cap_lock);
+       return count;
+}
+
+static struct kobj_attribute cap_state_attribute =
+       __ATTR(core_cap_state, 0644, core_cap_state_show, core_cap_state_store);
+static struct kobj_attribute cap_level_attribute =
+       __ATTR(core_cap_level, 0644, core_cap_level_show, core_cap_level_store);
+
+const struct attribute *cap_attributes[] = {
+       &cap_state_attribute.attr,
+       &cap_level_attribute.attr,
+       NULL,
+};
+
+void tegra_dvfs_core_cap_enable(bool enable)
+{
+       mutex_lock(&core_cap_lock);
+
+       if (enable) {
+               kdvfs_core_cap.refcnt++;
+               if (kdvfs_core_cap.refcnt == 1)
+                       core_cap_enable(true);
+       } else if (kdvfs_core_cap.refcnt) {
+               kdvfs_core_cap.refcnt--;
+               if (kdvfs_core_cap.refcnt == 0)
+                       core_cap_enable(false);
+       }
+       mutex_unlock(&core_cap_lock);
+}
+
+void tegra_dvfs_core_cap_level_set(int level)
+{
+       mutex_lock(&core_cap_lock);
+       kdvfs_core_cap.level = level;
+       core_cap_update();
+       mutex_unlock(&core_cap_lock);
+}
+
+static int __init init_core_cap_one(struct clk *c, unsigned long *freqs)
+{
+       int i, v, next_v = 0;
+       unsigned long rate, next_rate = 0;
+
+       for (i = 0; i < cap_millivolts_num; i++) {
+               v = cap_millivolts[i];
+               if (v == 0)
+                       break;
+
+               for (;;) {
+                       rate = next_rate;
+                       next_rate = clk_round_rate(c->parent, rate + 1000);
+                       if (IS_ERR_VALUE(next_rate)) {
+                               pr_debug("tegra11_dvfs: failed to round %s rate %lu\n",
+                                        c->name, rate);
+                               return -EINVAL;
+                       }
+                       if (rate == next_rate)
+                               break;
+
+                       next_v = tegra_dvfs_predict_millivolts(
+                               c->parent, next_rate);
+                       if (IS_ERR_VALUE(next_v)) {
+                               pr_debug("tegra11_dvfs: failed to predict %s mV for rate %lu\n",
+                                        c->name, next_rate);
+                               return -EINVAL;
+                       }
+                       if (next_v > v)
+                               break;
+               }
+
+               if (rate == 0) {
+                       rate = next_rate;
+                       pr_warn("tegra11_dvfs: minimum %s rate %lu requires %d mV\n",
+                               c->name, rate, next_v);
+               }
+               freqs[i] = rate;
+               next_rate = rate;
+       }
+       return 0;
+}
+
+int __init tegra_init_core_cap(
+       struct core_dvfs_cap_table *table, int table_size,
+       const int *millivolts, int millivolts_num,
+       struct kobject *cap_kobj)
+{
+       int i;
+       struct clk *c = NULL;
+
+       if (!table || !table_size || !millivolts || !millivolts_num)
+               return -EINVAL;
+
+       core_nominal_mv =
+               tegra_dvfs_rail_get_nominal_millivolts(tegra_core_rail);
+       if (core_nominal_mv <= 0)
+               return -ENODATA;
+
+       cap_millivolts = millivolts;
+       cap_millivolts_num = millivolts_num;
+       core_buses_cap.level = kdvfs_core_cap.level = user_core_cap.level =
+               core_nominal_mv;
+
+       for (i = 0; i < table_size; i++) {
+               c = tegra_get_clock_by_name(table[i].cap_name);
+               if (!c || !c->parent ||
+                   init_core_cap_one(c, table[i].freqs)) {
+                       pr_err("%s: failed to initialize %s table\n",
+                              __func__, table[i].cap_name);
+                       continue;
+               }
+               table[i].cap_clk = c;
+       }
+
+       if (!cap_kobj || sysfs_create_files(cap_kobj, cap_attributes))
+               return -ENOMEM;
+
+       core_cap_table = table;
+       core_cap_table_size = table_size;
+       return 0;
+}
+