Merge tag 'v3.14.25' into backport/v3.14.24-ltsi-rc1+v3.14.25/snapshot-merge.wip

[platform/adaptation/renesas_rcar/renesas_kernel.git] / drivers / sh / clk / cpg.c

diff --git a/drivers/sh/clk/cpg.c b/drivers/sh/clk/cpg.c
index 1ebe67c..7442bc1 100644 (file)
--- a/drivers/sh/clk/cpg.c
+++ b/drivers/sh/clk/cpg.c
@@ -36,9 +36,47 @@ static void sh_clk_write(int value, struct clk *clk)
                iowrite32(value, clk->mapped_reg);
 }
 
+static unsigned int r8(const void __iomem *addr)
+{
+       return ioread8(addr);
+}
+
+static unsigned int r16(const void __iomem *addr)
+{
+       return ioread16(addr);
+}
+
+static unsigned int r32(const void __iomem *addr)
+{
+       return ioread32(addr);
+}
+
 static int sh_clk_mstp_enable(struct clk *clk)
 {
        sh_clk_write(sh_clk_read(clk) & ~(1 << clk->enable_bit), clk);
+       if (clk->status_reg) {
+               unsigned int (*read)(const void __iomem *addr);
+               int i;
+               void __iomem *mapped_status = (phys_addr_t)clk->status_reg -
+                       (phys_addr_t)clk->enable_reg + clk->mapped_reg;
+
+               if (clk->flags & CLK_ENABLE_REG_8BIT)
+                       read = r8;
+               else if (clk->flags & CLK_ENABLE_REG_16BIT)
+                       read = r16;
+               else
+                       read = r32;
+
+               for (i = 1000;
+                    (read(mapped_status) & (1 << clk->enable_bit)) && i;
+                    i--)
+                       cpu_relax();
+               if (!i) {
+                       pr_err("cpg: failed to enable %p[%d]\n",
+                              clk->enable_reg, clk->enable_bit);
+                       return -ETIMEDOUT;
+               }
+       }
        return 0;
 }