1 files changed, 44 insertions, 4 deletions

diff --git a/drivers/clk/shmobile/clk-rcar-gen2.c b/drivers/clk/shmobile/clk-rcar-gen2.c
index a59ec217a124..99c27b1c625b 100644
--- a/drivers/clk/shmobile/clk-rcar-gen2.c
+++ b/drivers/clk/shmobile/clk-rcar-gen2.c
@@ -26,6 +26,8 @@ struct rcar_gen2_cpg {
         void __iomem *reg;
 };
 
+#define CPG_FRQCRB                      0x00000004
+#define CPG_FRQCRB_KICK                 BIT(31)
 #define CPG_SDCKCR                      0x00000074
 #define CPG_PLL0CR                      0x000000d8
 #define CPG_FRQCRC                      0x000000e0
@@ -45,6 +47,7 @@ struct rcar_gen2_cpg {
 struct cpg_z_clk {
         struct clk_hw hw;
         void __iomem *reg;
+        void __iomem *kick_reg;
 };
 
 #define to_z_clk(_hw)   container_of(_hw, struct cpg_z_clk, hw)
@@ -83,17 +86,45 @@ static int cpg_z_clk_set_rate(struct clk_hw *hw, unsigned long rate,
 {
         struct cpg_z_clk *zclk = to_z_clk(hw);
         unsigned int mult;
-        u32 val;
+        u32 val, kick;
+        unsigned int i;
 
         mult = div_u64((u64)rate * 32, parent_rate);
         mult = clamp(mult, 1U, 32U);
 
+        if (clk_readl(zclk->kick_reg) & CPG_FRQCRB_KICK)
+                return -EBUSY;
+
         val = clk_readl(zclk->reg);
         val &= ~CPG_FRQCRC_ZFC_MASK;
         val |= (32 - mult) << CPG_FRQCRC_ZFC_SHIFT;
         clk_writel(val, zclk->reg);
 
-        return 0;
+        /*
+         * Set KICK bit in FRQCRB to update hardware setting and wait for
+         * clock change completion.
+         */
+        kick = clk_readl(zclk->kick_reg);
+        kick |= CPG_FRQCRB_KICK;
+        clk_writel(kick, zclk->kick_reg);
+
+        /*
+         * Note: There is no HW information about the worst case latency.
+         *
+         * Using experimental measurements, it seems that no more than
+         * ~10 iterations are needed, independently of the CPU rate.
+         * Since this value might be dependant of external xtal rate, pll1
+         * rate or even the other emulation clocks rate, use 1000 as a
+         * "super" safe value.
+         */
+        for (i = 1000; i; i--) {
+                if (!(clk_readl(zclk->kick_reg) & CPG_FRQCRB_KICK))
+                        return 0;
+
+                cpu_relax();
+        }
+
+        return -ETIMEDOUT;
 }
 
 static const struct clk_ops cpg_z_clk_ops = {
@@ -120,6 +151,7 @@ static struct clk * __init cpg_z_clk_register(struct rcar_gen2_cpg *cpg)
         init.num_parents = 1;
 
         zclk->reg = cpg->reg + CPG_FRQCRC;
+        zclk->kick_reg = cpg->reg + CPG_FRQCRB;
         zclk->hw.init = &init;
 
         clk = clk_register(NULL, &zclk->hw);
@@ -186,7 +218,7 @@ rcar_gen2_cpg_register_clock(struct device_node *np, struct rcar_gen2_cpg *cpg,
                              const char *name)
 {
         const struct clk_div_table *table = NULL;
-        const char *parent_name = "main";
+        const char *parent_name;
         unsigned int shift;
         unsigned int mult = 1;
         unsigned int div = 1;
@@ -201,23 +233,31 @@ rcar_gen2_cpg_register_clock(struct device_node *np, struct rcar_gen2_cpg *cpg,
                  * the multiplier value.
                  */
                 u32 value = clk_readl(cpg->reg + CPG_PLL0CR);
+                parent_name = "main";
                 mult = ((value >> 24) & ((1 << 7) - 1)) + 1;
         } else if (!strcmp(name, "pll1")) {
+                parent_name = "main";
                 mult = config->pll1_mult / 2;
         } else if (!strcmp(name, "pll3")) {
+                parent_name = "main";
                 mult = config->pll3_mult;
         } else if (!strcmp(name, "lb")) {
+                parent_name = "pll1_div2";
                 div = cpg_mode & BIT(18) ? 36 : 24;
         } else if (!strcmp(name, "qspi")) {
+                parent_name = "pll1_div2";
                 div = (cpg_mode & (BIT(3) | BIT(2) | BIT(1))) == BIT(2)
-                    ? 16 : 20;
+                    ? 8 : 10;
         } else if (!strcmp(name, "sdh")) {
+                parent_name = "pll1_div2";
                 table = cpg_sdh_div_table;
                 shift = 8;
         } else if (!strcmp(name, "sd0")) {
+                parent_name = "pll1_div2";
                 table = cpg_sd01_div_table;
                 shift = 4;
         } else if (!strcmp(name, "sd1")) {
+                parent_name = "pll1_div2";
                 table = cpg_sd01_div_table;
                 shift = 0;
         } else if (!strcmp(name, "z")) {