1 files changed, 38 insertions, 0 deletions

diff --git a/drivers/sh/clk/cpg.c b/drivers/sh/clk/cpg.c
index 1ebe67cd1833..7442bc130055 100644
--- 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;
 }