Merge branch 'linus' into core/softlockup

[mirror_ubuntu-hirsute-kernel.git] / arch / arm / mach-imx / cpufreq.c

diff --git a/arch/arm/mach-imx/cpufreq.c b/arch/arm/mach-imx/cpufreq.c
index 4f66e90db74f934d7db83b00de0a824b0df1cd48..be0809b33e089af99094af02f8e0ce509ec18a49 100644 (file)
--- a/arch/arm/mach-imx/cpufreq.c
+++ b/arch/arm/mach-imx/cpufreq.c
@@ -32,6 +32,8 @@
 #include <linux/types.h>
 #include <linux/init.h>
 #include <linux/cpufreq.h>
+#include <linux/clk.h>
+#include <linux/err.h>
 #include <asm/system.h>
 
 #include <asm/hardware.h>
@@ -50,6 +52,9 @@
 #define CR_920T_ASYNC_MODE     0xC0000000
 
 static u32 mpctl0_at_boot;
+static u32 bclk_div_at_boot;
+
+static struct clk *system_clk, *mcu_clk;
 
 static void imx_set_async_mode(void)
 {
@@ -82,13 +87,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;
@@ -159,10 +164,10 @@ static unsigned int imx_get_speed(unsigned int cpu)
        cr = get_cr();
 
        if((cr & CR_920T_CLOCK_MODE) == CR_920T_FASTBUS_MODE) {
-               freq = imx_get_system_clk();
+               freq = clk_get_rate(system_clk);
                freq = (freq + bclk_div/2) / bclk_div;
        } else {
-               freq = imx_get_mcu_clk();
+               freq = clk_get_rate(mcu_clk);
                if (cscr & CSCR_MPU_PRESC)
                        freq /= 2;
        }
@@ -182,7 +187,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
@@ -200,10 +205,10 @@ static int imx_set_target(struct cpufreq_policy *policy,
        pr_debug(KERN_DEBUG "imx: requested frequency %ld Hz, mpctl0 at boot 0x%08x\n",
                        freq, mpctl0_at_boot);
 
-       sysclk = imx_get_system_clk();
+       sysclk = clk_get_rate(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 +222,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;
        }
@@ -242,7 +249,7 @@ static int imx_set_target(struct cpufreq_policy *policy,
        if(mpctl0) {
                CSCR |= CSCR_MPLL_RESTART;
 
-               /* Wait until MPLL is stablized */
+               /* Wait until MPLL is stabilized */
                while( CSCR & CSCR_MPLL_RESTART );
 
                imx_set_async_mode();
@@ -266,7 +273,6 @@ static int __init imx_cpufreq_driver_init(struct cpufreq_policy *policy)
                return -EINVAL;
 
        policy->cur = policy->min = policy->max = imx_get_speed(0);
-       policy->governor = CPUFREQ_DEFAULT_GOVERNOR;
        policy->cpuinfo.min_freq = 8000;
        policy->cpuinfo.max_freq = 200000;
         /* Manual states, that PLL stabilizes in two CLK32 periods */
@@ -285,9 +291,19 @@ 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;
 
+       system_clk = clk_get(NULL, "system_clk");
+       if (IS_ERR(system_clk))
+               return PTR_ERR(system_clk);
+
+       mcu_clk = clk_get(NULL, "mcu_clk");
+       if (IS_ERR(mcu_clk)) {
+               clk_put(system_clk);
+               return PTR_ERR(mcu_clk);
+       }
+
        if((CSCR & CSCR_MPEN) &&
           ((get_cr() & CR_920T_CLOCK_MODE) != CR_920T_FASTBUS_MODE))
                mpctl0_at_boot = MPCTL0;