1 files changed, 16 insertions, 20 deletions

diff --git a/drivers/cpufreq/exynos5440-cpufreq.c b/drivers/cpufreq/exynos5440-cpufreq.c
index 76bef8b078cb..49b756015316 100644
--- a/drivers/cpufreq/exynos5440-cpufreq.c
+++ b/drivers/cpufreq/exynos5440-cpufreq.c
@@ -100,7 +100,6 @@ struct exynos_dvfs_data {
         struct resource *mem;
         int irq;
         struct clk *cpu_clk;
-        unsigned int cur_frequency;
         unsigned int latency;
         struct cpufreq_frequency_table *freq_table;
         unsigned int freq_count;
@@ -165,7 +164,7 @@ static int init_div_table(void)
         return 0;
 }
 
-static void exynos_enable_dvfs(void)
+static void exynos_enable_dvfs(unsigned int cur_frequency)
 {
         unsigned int tmp, i, cpu;
         struct cpufreq_frequency_table *freq_table = dvfs_info->freq_table;
@@ -184,18 +183,18 @@ static void exynos_enable_dvfs(void)
 
         /* Set initial performance index */
         for (i = 0; freq_table[i].frequency != CPUFREQ_TABLE_END; i++)
-                if (freq_table[i].frequency == dvfs_info->cur_frequency)
+                if (freq_table[i].frequency == cur_frequency)
                         break;
 
         if (freq_table[i].frequency == CPUFREQ_TABLE_END) {
                 dev_crit(dvfs_info->dev, "Boot up frequency not supported\n");
                 /* Assign the highest frequency */
                 i = 0;
-                dvfs_info->cur_frequency = freq_table[i].frequency;
+                cur_frequency = freq_table[i].frequency;
         }
 
         dev_info(dvfs_info->dev, "Setting dvfs initial frequency = %uKHZ",
-                                                dvfs_info->cur_frequency);
+                                                cur_frequency);
 
         for (cpu = 0; cpu < CONFIG_NR_CPUS; cpu++) {
                 tmp = __raw_readl(dvfs_info->base + XMU_C0_3_PSTATE + cpu * 4);
@@ -209,11 +208,6 @@ static void exynos_enable_dvfs(void)
                                 dvfs_info->base + XMU_DVFS_CTRL);
 }
 
-static unsigned int exynos_getspeed(unsigned int cpu)
-{
-        return dvfs_info->cur_frequency;
-}
-
 static int exynos_target(struct cpufreq_policy *policy, unsigned int index)
 {
         unsigned int tmp;
@@ -222,7 +216,7 @@ static int exynos_target(struct cpufreq_policy *policy, unsigned int index)
 
         mutex_lock(&cpufreq_lock);
 
-        freqs.old = dvfs_info->cur_frequency;
+        freqs.old = policy->cur;
         freqs.new = freq_table[index].frequency;
 
         cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
@@ -250,7 +244,7 @@ static void exynos_cpufreq_work(struct work_struct *work)
                 goto skip_work;
 
         mutex_lock(&cpufreq_lock);
-        freqs.old = dvfs_info->cur_frequency;
+        freqs.old = policy->cur;
 
         cur_pstate = __raw_readl(dvfs_info->base + XMU_P_STATUS);
         if (cur_pstate >> C0_3_PSTATE_VALID_SHIFT & 0x1)
@@ -260,10 +254,9 @@ static void exynos_cpufreq_work(struct work_struct *work)
 
         if (likely(index < dvfs_info->freq_count)) {
                 freqs.new = freq_table[index].frequency;
-                dvfs_info->cur_frequency = freqs.new;
         } else {
                 dev_crit(dvfs_info->dev, "New frequency out of range\n");
-                freqs.new = dvfs_info->cur_frequency;
+                freqs.new = freqs.old;
         }
         cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
@@ -307,15 +300,17 @@ static void exynos_sort_descend_freq_table(void)
 
 static int exynos_cpufreq_cpu_init(struct cpufreq_policy *policy)
 {
+        policy->clk = dvfs_info->cpu_clk;
         return cpufreq_generic_init(policy, dvfs_info->freq_table,
                         dvfs_info->latency);
 }
 
 static struct cpufreq_driver exynos_driver = {
-        .flags          = CPUFREQ_STICKY | CPUFREQ_ASYNC_NOTIFICATION,
+        .flags          = CPUFREQ_STICKY | CPUFREQ_ASYNC_NOTIFICATION |
+                                CPUFREQ_NEED_INITIAL_FREQ_CHECK,
         .verify         = cpufreq_generic_frequency_table_verify,
         .target_index   = exynos_target,
-        .get            = exynos_getspeed,
+        .get            = cpufreq_generic_get,
         .init           = exynos_cpufreq_cpu_init,
         .exit           = cpufreq_generic_exit,
         .name           = CPUFREQ_NAME,
@@ -335,6 +330,7 @@ static int exynos_cpufreq_probe(struct platform_device *pdev)
         int ret = -EINVAL;
         struct device_node *np;
         struct resource res;
+        unsigned int cur_frequency;
 
         np =  pdev->dev.of_node;
         if (!np)
@@ -391,13 +387,13 @@ static int exynos_cpufreq_probe(struct platform_device *pdev)
                 goto err_free_table;
         }
 
-        dvfs_info->cur_frequency = clk_get_rate(dvfs_info->cpu_clk);
-        if (!dvfs_info->cur_frequency) {
+        cur_frequency = clk_get_rate(dvfs_info->cpu_clk);
+        if (!cur_frequency) {
                 dev_err(dvfs_info->dev, "Failed to get clock rate\n");
                 ret = -EINVAL;
                 goto err_free_table;
         }
-        dvfs_info->cur_frequency /= 1000;
+        cur_frequency /= 1000;
 
         INIT_WORK(&dvfs_info->irq_work, exynos_cpufreq_work);
         ret = devm_request_irq(dvfs_info->dev, dvfs_info->irq,
@@ -414,7 +410,7 @@ static int exynos_cpufreq_probe(struct platform_device *pdev)
                 goto err_free_table;
         }
 
-        exynos_enable_dvfs();
+        exynos_enable_dvfs(cur_frequency);
         ret = cpufreq_register_driver(&exynos_driver);
         if (ret) {
                 dev_err(dvfs_info->dev,