Merge branch 'linux-2.6' into for-2.6.22

2 files changed, 14 insertions, 10 deletions

diff --git a/arch/arm/mach-imx/cpufreq.c b/arch/arm/mach-imx/cpufreq.c
index 4f66e90db74f..7e70e0b0b989 100644
--- a/arch/arm/mach-imx/cpufreq.c
+++ b/arch/arm/mach-imx/cpufreq.c
@@ -50,6 +50,7 @@
 #define CR_920T_ASYNC_MODE      0xC0000000
 
 static u32 mpctl0_at_boot;
+static u32 bclk_div_at_boot;
 
 static void imx_set_async_mode(void)
 {
@@ -82,13 +83,13 @@ static void imx_set_mpctl0(u32 mpctl0)
  * imx_compute_mpctl - compute new PLL parameters
  * @new_mpctl:  pointer to location assigned by new PLL control register value
  * @cur_mpctl:  current PLL control register parameters
+ * @f_ref:      reference source frequency Hz
  * @freq:       required frequency in Hz
  * @relation:   is one of %CPUFREQ_RELATION_L (supremum)
  *              and %CPUFREQ_RELATION_H (infimum)
  */
-long imx_compute_mpctl(u32 *new_mpctl, u32 cur_mpctl, unsigned long freq, int relation)
+long imx_compute_mpctl(u32 *new_mpctl, u32 cur_mpctl, u32 f_ref, unsigned long freq, int relation)
 {
-        u32 f_ref = (CSCR & CSCR_SYSTEM_SEL) ? 16000000 : (CLK32 * 512);
         u32 mfi;
         u32 mfn;
         u32 mfd;
@@ -182,7 +183,7 @@ static int imx_set_target(struct cpufreq_policy *policy,
         unsigned long flags;
         long freq;
         long sysclk;
-        unsigned int bclk_div = 1;
+        unsigned int bclk_div = bclk_div_at_boot;
 
         /*
          * Some governors do not respects CPU and policy lower limits
@@ -202,8 +203,8 @@ static int imx_set_target(struct cpufreq_policy *policy,
 
         sysclk = imx_get_system_clk();
 
-        if (freq > sysclk + 1000000) {
-                freq = imx_compute_mpctl(&mpctl0, mpctl0_at_boot, freq, relation);
+        if (freq > sysclk / bclk_div_at_boot + 1000000) {
+                freq = imx_compute_mpctl(&mpctl0, mpctl0_at_boot, CLK32 * 512, freq, relation);
                 if (freq < 0) {
                         printk(KERN_WARNING "imx: target frequency %ld Hz cannot be set\n", freq);
                         return -EINVAL;
@@ -217,6 +218,8 @@ static int imx_set_target(struct cpufreq_policy *policy,
 
                         if(bclk_div > 16)
                                 bclk_div = 16;
+                        if(bclk_div < bclk_div_at_boot)
+                                bclk_div = bclk_div_at_boot;
                 }
                 freq = (sysclk + bclk_div / 2) / bclk_div;
         }
@@ -285,7 +288,7 @@ static struct cpufreq_driver imx_driver = {
 
 static int __init imx_cpufreq_init(void)
 {
-
+        bclk_div_at_boot = __mfld2val(CSCR_BCLK_DIV, CSCR) + 1;
         mpctl0_at_boot = 0;
 
         if((CSCR & CSCR_MPEN) &&
diff --git a/arch/arm/mach-imx/generic.c b/arch/arm/mach-imx/generic.c
index b5aa49d00ca3..7a7fa51ec62c 100644
--- a/arch/arm/mach-imx/generic.c
+++ b/arch/arm/mach-imx/generic.c
@@ -102,7 +102,7 @@ EXPORT_SYMBOL(imx_gpio_mode);
  *  f = 2 * f_ref * --------------------
  *                        pd + 1
  */
-static unsigned int imx_decode_pll(unsigned int pll)
+static unsigned int imx_decode_pll(unsigned int pll, u32 f_ref)
 {
         unsigned long long ll;
         unsigned long quot;
@@ -111,7 +111,6 @@ static unsigned int imx_decode_pll(unsigned int pll)
         u32 mfn = pll & 0x3ff;
         u32 mfd = (pll >> 16) & 0x3ff;
         u32 pd =  (pll >> 26) & 0xf;
-        u32 f_ref = (CSCR & CSCR_SYSTEM_SEL) ? 16000000 : (CLK32 * 512);
 
         mfi = mfi <= 5 ? 5 : mfi;
 
@@ -124,13 +123,15 @@ static unsigned int imx_decode_pll(unsigned int pll)
 
 unsigned int imx_get_system_clk(void)
 {
-        return imx_decode_pll(SPCTL0);
+        u32 f_ref = (CSCR & CSCR_SYSTEM_SEL) ? 16000000 : (CLK32 * 512);
+
+        return imx_decode_pll(SPCTL0, f_ref);
 }
 EXPORT_SYMBOL(imx_get_system_clk);
 
 unsigned int imx_get_mcu_clk(void)
 {
-        return imx_decode_pll(MPCTL0);
+        return imx_decode_pll(MPCTL0, CLK32 * 512);
 }
 EXPORT_SYMBOL(imx_get_mcu_clk);