diff --git a/arch/arm/mach-shmobile/clock-sh73a0.c b/arch/arm/mach-shmobile/clock-sh73a0.c index d05cf9039788..d9fd0336b910 100644 --- a/arch/arm/mach-shmobile/clock-sh73a0.c +++ b/arch/arm/mach-shmobile/clock-sh73a0.c @@ -257,7 +257,7 @@ static struct clk twd_clk = { .ops = &twd_clk_ops, }; -static struct sh_clk_ops zclk_ops; +static struct sh_clk_ops zclk_ops, kicker_ops; static const struct sh_clk_ops *div4_clk_ops; static int zclk_set_rate(struct clk *clk, unsigned long rate) @@ -324,18 +324,32 @@ static unsigned long zclk_recalc(struct clk *clk) return clk_get_rate(clk->parent); } -static void zclk_extend(void) +static int kicker_set_rate(struct clk *clk, unsigned long rate) { - div4_clk_ops = div4_clks[DIV4_Z].ops; + if (__raw_readl(FRQCRB) & (1 << 31)) + return -EBUSY; + return div4_clk_ops->set_rate(clk, rate); +} + +static void div4_clk_extend(void) +{ + int i; + + div4_clk_ops = div4_clks[0].ops; + + /* Add a kicker-busy check before changing the rate */ + kicker_ops = *div4_clk_ops; /* We extend the DIV4 clock with a 1:1 pass-through case */ zclk_ops = *div4_clk_ops; + kicker_ops.set_rate = kicker_set_rate; zclk_ops.set_rate = zclk_set_rate; zclk_ops.round_rate = zclk_round_rate; zclk_ops.recalc = zclk_recalc; - div4_clks[DIV4_Z].ops = &zclk_ops; + for (i = 0; i < DIV4_NR; i++) + div4_clks[i].ops = i == DIV4_Z ? &zclk_ops : &kicker_ops; } enum { DIV6_VCK1, DIV6_VCK2, DIV6_VCK3, DIV6_ZB1, @@ -697,7 +711,7 @@ void __init sh73a0_clock_init(void) if (!ret) { ret = sh_clk_div4_register(div4_clks, DIV4_NR, &div4_table); if (!ret) - zclk_extend(); + div4_clk_extend(); } if (!ret)