ENGR00170340 [MX6] Fix incorrect frequencies reported from cpuinfo

Fix incorrect frequencies reported from /proc/cpuinfo.

Signed-off-by: Nancy Chen <Nancy.Chen@freescale.com>

diff --git a/arch/arm/mach-mx6/cpu_regulator-mx6.c b/arch/arm/mach-mx6/cpu_regulator-mx6.c
index 35f86a9cd473875c56698c443c1b2de0cf6dfb1a..f973c5ae4f9b03e4296f925ec9170ad07b0f217c 100644 (file)
--- a/arch/arm/mach-mx6/cpu_regulator-mx6.c
+++ b/arch/arm/mach-mx6/cpu_regulator-mx6.c
@@ -14,9 +14,12 @@
 #include <linux/types.h>
 #include <linux/kernel.h>
 #include <linux/err.h>
+#include <linux/jiffies.h>
 #include <linux/regulator/consumer.h>
 #include <linux/clk.h>
-
+#if defined(CONFIG_CPU_FREQ)
+#include <linux/cpufreq.h>
+#endif
 #include <asm/cpu.h>
 
 #include <mach/clock.h>
@@ -29,9 +32,30 @@ static int cpu_op_nr;
 static struct cpu_op *cpu_op_tbl;
 extern struct cpu_op *(*get_cpu_op)(int *op);
 
+static inline unsigned long mx6_cpu_jiffies(unsigned long old, u_int div,
+                                             u_int mult)
+{
+#if BITS_PER_LONG == 32
+
+       u64 result = ((u64) old) * ((u64) mult);
+       do_div(result, div);
+       return (unsigned long) result;
+
+#elif BITS_PER_LONG == 64
+
+       unsigned long result = old * ((u64) mult);
+       result /= div;
+       return result;
+
+#endif
+}
+
 
 void mx6_cpu_regulator_init(void)
 {
+       int cpu;
+       u32 curr_cpu = 0;
+
        cpu_regulator = regulator_get(NULL, gp_reg_id);
        if (IS_ERR(cpu_regulator))
                printk(KERN_ERR "%s: failed to get cpu regulator\n", __func__);
@@ -41,12 +65,35 @@ void mx6_cpu_regulator_init(void)
                        printk(KERN_ERR "%s: failed to get cpu clock\n",
                               __func__);
                } else {
+                       curr_cpu = clk_get_rate(cpu_clk);
                        cpu_op_tbl = get_cpu_op(&cpu_op_nr);
                        /* Set the core to max frequency requested. */
                        regulator_set_voltage(cpu_regulator,
                                              cpu_op_tbl[0].cpu_voltage,
                                              cpu_op_tbl[0].cpu_voltage);
-                       clk_set_rate(cpu_clk, cpu_op_tbl[0].pll_rate);
+                       clk_set_rate(cpu_clk, cpu_op_tbl[0].cpu_rate);
+
+                       /*Fix loops-per-jiffy */
+#ifdef CONFIG_SMP
+                       for_each_online_cpu(cpu)
+                               per_cpu(cpu_data, cpu).loops_per_jiffy =
+                               mx6_cpu_jiffies(
+                                       per_cpu(cpu_data, cpu).loops_per_jiffy,
+                                       curr_cpu / 1000,
+                                       clk_get_rate(cpu_clk) / 1000);
+#else
+                       u32 old_loops_per_jiffy = loops_per_jiffy;
+
+                       loops_per_jiffy =
+                               mx6_cpu_jiffies(old_loops_per_jiffy,
+                                               curr_cpu/1000,
+                                               clk_get_rate(cpu_clk) / 1000);
+#endif
+#if defined(CONFIG_CPU_FREQ)
+                       /* Fix CPU frequency for CPUFREQ. */
+                       for (cpu = 0; cpu < num_online_cpus(); cpu++)
+                               cpufreq_get(cpu);
+#endif
                }
        }
 }