clk: m8b cpu_clk support more rates

PD#141217: m8b cpu_clk support more rates

Change-Id: I6ac361cd5e30fa3b51e2e58633952971d6e78206
Signed-off-by: Yun Cai <yun.cai@amlogic.com>

diff --git a/drivers/amlogic/clk/m8b/clk-cpu.c b/drivers/amlogic/clk/m8b/clk-cpu.c
index aba3fa1..fef94e4 100644 (file)
--- a/drivers/amlogic/clk/m8b/clk-cpu.c
+++ b/drivers/amlogic/clk/m8b/clk-cpu.c
@@ -54,50 +54,132 @@
 #define to_meson_clk_cpu_hw(_hw) container_of(_hw, struct meson_clk_cpu, hw)
 #define to_meson_clk_cpu_nb(_nb) container_of(_nb, struct meson_clk_cpu, clk_nb)
 
+struct hrtimer cpu_hrtimer;
+DEFINE_SPINLOCK(pll_changing);
+static int swing_inteval = 25000;
+static unsigned long long cnt;
+static unsigned int index;
+static unsigned long cpu_prate;
+static unsigned int *cpu_vbase;
+
+enum hrtimer_restart virtual_clock_work(struct hrtimer *hrtimer)
+{
+       unsigned long flag;
+       u32 reg;
+       static unsigned int bitmap[][4] = {
+               {1, 0, 1, 1},
+               {0, 1, 0, 1},
+               {0, 1, 0, 0}
+       };
+
+       if (!spin_trylock_irqsave(&pll_changing, flag))
+               return HRTIMER_NORESTART;
+
+       cnt++;
+       reg = readl(cpu_vbase);
+
+       if (bitmap[index-1][cnt&0x3]) {
+               writel((reg&(~(0x1<<7)))|(0x0<<7), cpu_vbase); /*xtal*/
+               reg = readl(cpu_vbase);
+               udelay(100);
+               writel((reg&(~(0x3<<2)))|(0x1<<2), cpu_vbase); /*sys_pll/2*/
+               reg = readl(cpu_vbase);
+               udelay(100);
+               writel((reg&(~(0x1<<7)))|(0x1<<7), cpu_vbase); /*sys_pll*/
+               pr_debug("MESON_CPU_CLK_CNTL: 0x%x\n", readl(cpu_vbase));
+       } else {
+               writel((reg&(~(0x1<<7)))|(0x0<<7), cpu_vbase); /*xtal*/
+               reg = readl(cpu_vbase);
+               udelay(100);
+               writel((reg&(~(0x3<<2)))|(0x0<<2), cpu_vbase); /*sys_pll*/
+               reg = readl(cpu_vbase);
+               udelay(100);
+               writel((reg&(~(0x1<<7)))|(0x1<<7), cpu_vbase); /*sys_pll*/
+               pr_debug("MESON_CPU_CLK_CNTL: 0x%x\n", readl(cpu_vbase));
+       }
+       hrtimer_start(&cpu_hrtimer, ktime_set(0, swing_inteval * 1000),
+               HRTIMER_MODE_REL);
+       spin_unlock_irqrestore(&pll_changing, flag);
+       return HRTIMER_NORESTART;
+}
+
 static long meson_clk_cpu_round_rate(struct clk_hw *hw, unsigned long rate,
                                     unsigned long *prate)
 {
        struct meson_clk_cpu *clk_cpu = to_meson_clk_cpu_hw(hw);
+       unsigned long long parent_rate = *prate;
+
+       index = 0;
 
-       return divider_round_rate(hw, rate, prate, clk_cpu->div_table,
+       if (rate <= parent_rate/2)
+               rate = divider_round_rate(hw, rate, prate, clk_cpu->div_table,
                                  MESON_N_WIDTH, CLK_DIVIDER_ROUND_CLOSEST);
+       else if (rate < parent_rate*5/8)
+               rate = parent_rate/2;
+       else if (rate < parent_rate*6/8) {
+               rate = parent_rate*5/8;
+               index = 1;
+       } else if (rate < parent_rate*7/8) {
+               rate = parent_rate*6/8;
+               index = 2;
+       } else if (rate < parent_rate) {
+               rate = parent_rate*7/8;
+               index = 3;
+       }
+
+       return rate;
 }
 
 static int meson_clk_cpu_set_rate(struct clk_hw *hw, unsigned long rate,
                                  unsigned long parent_rate)
 {
        struct meson_clk_cpu *clk_cpu = to_meson_clk_cpu_hw(hw);
-       unsigned int div, sel, N = 0;
+       unsigned int div;
        u32 reg, reg1, sel_first = 0;
 
        div = DIV_ROUND_UP(parent_rate, rate);
 
-       if (div <= 3) {
-               sel = div - 1;
-       } else {
-               sel = 3;
-               N = div / 2;
-       }
+       if ((div == 2) && (parent_rate != rate)) {
+               cpu_prate = parent_rate;
+               cpu_vbase = clk_cpu->base + clk_cpu->reg_off +
+                       MESON_CPU_CLK_CNTL;
 
-       reg = readl(clk_cpu->base + clk_cpu->reg_off + MESON_CPU_CLK_CNTL1);
-       reg = PARM_SET(MESON_N_WIDTH, MESON_N_SHIFT, reg, N);
-
-       reg1 = readl(clk_cpu->base + clk_cpu->reg_off + MESON_CPU_CLK_CNTL);
-       if ((N == 0) && (((reg1>>2)&0x3) == 0x3))
-               sel_first = 1;
+               hrtimer_start(&cpu_hrtimer, ktime_set(0, swing_inteval * 1000),
+                       HRTIMER_MODE_REL);
+       } else {
+               unsigned int sel, N = 0;
 
-       reg1 = PARM_SET(MESON_SEL_WIDTH, MESON_SEL_SHIFT, reg1, sel);
+               hrtimer_try_to_cancel(&cpu_hrtimer);
+               if (div <= 3) {
+                       sel = div - 1;
+               } else {
+                       sel = 3;
+                       N = div / 2;
+               }
 
-       if (sel_first) {
-               writel(reg1, clk_cpu->base + clk_cpu->reg_off +
-                       MESON_CPU_CLK_CNTL);
-               writel(reg, clk_cpu->base + clk_cpu->reg_off +
-                       MESON_CPU_CLK_CNTL1);
-       } else {
-               writel(reg, clk_cpu->base + clk_cpu->reg_off +
+               reg = readl(clk_cpu->base + clk_cpu->reg_off +
                        MESON_CPU_CLK_CNTL1);
-               writel(reg1, clk_cpu->base + clk_cpu->reg_off +
+               reg = PARM_SET(MESON_N_WIDTH, MESON_N_SHIFT, reg, N);
+
+               reg1 = readl(clk_cpu->base + clk_cpu->reg_off +
                        MESON_CPU_CLK_CNTL);
+               if (((N == 0) && (((reg1>>2)&0x3)) == 0x3))
+                       sel_first = 1;
+
+               reg1 = PARM_SET(MESON_SEL_WIDTH, MESON_SEL_SHIFT,
+                       reg1, sel);
+
+               if (sel_first) {
+                       writel(reg1, clk_cpu->base + clk_cpu->reg_off +
+                               MESON_CPU_CLK_CNTL);
+                       writel(reg, clk_cpu->base + clk_cpu->reg_off +
+                               MESON_CPU_CLK_CNTL1);
+               } else {
+                       writel(reg, clk_cpu->base + clk_cpu->reg_off +
+                               MESON_CPU_CLK_CNTL1);
+                       writel(reg1, clk_cpu->base + clk_cpu->reg_off +
+                               MESON_CPU_CLK_CNTL);
+               }
        }
 
        return 0;

diff --git a/drivers/amlogic/clk/m8b/clkc.h b/drivers/amlogic/clk/m8b/clkc.h
index 8ec0718..c05d98d 100644 (file)
--- a/drivers/amlogic/clk/m8b/clkc.h
+++ b/drivers/amlogic/clk/m8b/clkc.h
@@ -18,6 +18,8 @@
 #ifndef __CLKC_H
 #define __CLKC_H
 
+#include <linux/hrtimer.h>
+
 #define CLK_PARENT_ALTERNATE BIT(5)
 #define PMASK(width)                   GENMASK(width - 1, 0)
 #define SETPMASK(width, shift)         GENMASK(shift + width - 1, shift)
@@ -123,8 +125,10 @@ extern const struct clk_ops meson_clk_mux_ops;
 extern spinlock_t clk_lock;
 extern void __iomem *clk_base;
 extern struct clk **clks;
+extern struct hrtimer cpu_hrtimer;
 void amlogic_init_store(void);
 void amlogic_init_gpu(void);
 void amlogic_init_media(void);
 void amlogic_init_misc(void);
+enum hrtimer_restart virtual_clock_work(struct hrtimer *hrtimer);
 #endif /* __CLKC_H */

diff --git a/drivers/amlogic/clk/m8b/meson8b.c b/drivers/amlogic/clk/m8b/meson8b.c
index 8b98132..45223cd 100644 (file)
--- a/drivers/amlogic/clk/m8b/meson8b.c
+++ b/drivers/amlogic/clk/m8b/meson8b.c
@@ -776,6 +776,8 @@ static void __init meson8b_clkc_init(struct device_node *np)
                goto iounmap;
        }
        pr_debug("%s: cpu clk register notifier ok!", __func__);
+       hrtimer_init(&cpu_hrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+       cpu_hrtimer.function = virtual_clock_work;
 
        ret = of_clk_add_provider(np, of_clk_src_onecell_get,
                        &clk_data);