diff options
Diffstat (limited to 'drivers/sh/clk/cpg.c')
| -rw-r--r-- | drivers/sh/clk/cpg.c | 35 |
1 files changed, 35 insertions, 0 deletions
diff --git a/drivers/sh/clk/cpg.c b/drivers/sh/clk/cpg.c index 82dd6fb1783..5e4301b936e 100644 --- a/drivers/sh/clk/cpg.c +++ b/drivers/sh/clk/cpg.c @@ -167,6 +167,38 @@ static struct clk_ops sh_clk_div6_reparent_clk_ops = { .set_parent = sh_clk_div6_set_parent, }; +static int __init sh_clk_init_parent(struct clk *clk) +{ + u32 val; + + if (clk->parent) + return 0; + + if (!clk->parent_table || !clk->parent_num) + return 0; + + if (!clk->src_width) { + pr_err("sh_clk_init_parent: cannot select parent clock\n"); + return -EINVAL; + } + + val = (__raw_readl(clk->enable_reg) >> clk->src_shift); + val &= (1 << clk->src_width) - 1; + + if (val >= clk->parent_num) { + pr_err("sh_clk_init_parent: parent table size failed\n"); + return -EINVAL; + } + + clk->parent = clk->parent_table[val]; + if (!clk->parent) { + pr_err("sh_clk_init_parent: unable to set parent"); + return -EINVAL; + } + + return 0; +} + static int __init sh_clk_div6_register_ops(struct clk *clks, int nr, struct clk_ops *ops) { @@ -190,6 +222,9 @@ static int __init sh_clk_div6_register_ops(struct clk *clks, int nr, clkp->ops = ops; clkp->freq_table = freq_table + (k * freq_table_size); clkp->freq_table[nr_divs].frequency = CPUFREQ_TABLE_END; + ret = sh_clk_init_parent(clkp); + if (ret < 0) + break; ret = clk_register(clkp); } |