Wolfram Sang | e848c2e | 2018-08-22 00:02:14 +0200 | [diff] [blame] | 1 | // SPDX-License-Identifier: GPL-2.0 |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 2 | /* |
| 3 | * r8a7790 Common Clock Framework support |
| 4 | * |
| 5 | * Copyright (C) 2013 Renesas Solutions Corp. |
| 6 | * |
| 7 | * Contact: Laurent Pinchart <laurent.pinchart@ideasonboard.com> |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 8 | */ |
| 9 | |
| 10 | #include <linux/clk-provider.h> |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 11 | #include <linux/init.h> |
| 12 | #include <linux/io.h> |
| 13 | #include <linux/kernel.h> |
Geert Uytterhoeven | 9f8c71e | 2017-06-21 22:34:33 +0200 | [diff] [blame] | 14 | #include <linux/notifier.h> |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 15 | #include <linux/of.h> |
| 16 | #include <linux/of_address.h> |
Geert Uytterhoeven | 9f8c71e | 2017-06-21 22:34:33 +0200 | [diff] [blame] | 17 | #include <linux/pm.h> |
Geert Uytterhoeven | 5a1cfaf | 2015-06-23 15:09:27 +0200 | [diff] [blame] | 18 | #include <linux/slab.h> |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 19 | |
Geert Uytterhoeven | 1fae91ec9 | 2015-10-16 11:41:19 +0200 | [diff] [blame] | 20 | #include "clk-div6.h" |
| 21 | |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 22 | #define CPG_DIV6_CKSTP BIT(8) |
| 23 | #define CPG_DIV6_DIV(d) ((d) & 0x3f) |
| 24 | #define CPG_DIV6_DIV_MASK 0x3f |
| 25 | |
| 26 | /** |
Wolfram Sang | 95aa4f9 | 2014-02-24 20:57:11 +0100 | [diff] [blame] | 27 | * struct div6_clock - CPG 6 bit divider clock |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 28 | * @hw: handle between common and hardware-specific interfaces |
| 29 | * @reg: IO-remapped register |
| 30 | * @div: divisor value (1-64) |
Geert Uytterhoeven | 23b04c8 | 2021-04-01 15:01:35 +0200 | [diff] [blame] | 31 | * @src_mask: Bitmask covering the register bits to select the parent clock |
Geert Uytterhoeven | 9f8c71e | 2017-06-21 22:34:33 +0200 | [diff] [blame] | 32 | * @nb: Notifier block to save/restore clock state for system resume |
Geert Uytterhoeven | ba03861 | 2019-06-12 17:22:18 +0200 | [diff] [blame] | 33 | * @parents: Array to map from valid parent clocks indices to hardware indices |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 34 | */ |
| 35 | struct div6_clock { |
| 36 | struct clk_hw hw; |
| 37 | void __iomem *reg; |
| 38 | unsigned int div; |
Geert Uytterhoeven | 23b04c8 | 2021-04-01 15:01:35 +0200 | [diff] [blame] | 39 | u32 src_mask; |
Geert Uytterhoeven | 9f8c71e | 2017-06-21 22:34:33 +0200 | [diff] [blame] | 40 | struct notifier_block nb; |
Geert Uytterhoeven | ba03861 | 2019-06-12 17:22:18 +0200 | [diff] [blame] | 41 | u8 parents[]; |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 42 | }; |
| 43 | |
| 44 | #define to_div6_clock(_hw) container_of(_hw, struct div6_clock, hw) |
| 45 | |
| 46 | static int cpg_div6_clock_enable(struct clk_hw *hw) |
| 47 | { |
| 48 | struct div6_clock *clock = to_div6_clock(hw); |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 49 | u32 val; |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 50 | |
Geert Uytterhoeven | c733c7d | 2018-03-15 10:43:12 +0100 | [diff] [blame] | 51 | val = (readl(clock->reg) & ~(CPG_DIV6_DIV_MASK | CPG_DIV6_CKSTP)) |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 52 | | CPG_DIV6_DIV(clock->div - 1); |
Geert Uytterhoeven | c733c7d | 2018-03-15 10:43:12 +0100 | [diff] [blame] | 53 | writel(val, clock->reg); |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 54 | |
| 55 | return 0; |
| 56 | } |
| 57 | |
| 58 | static void cpg_div6_clock_disable(struct clk_hw *hw) |
| 59 | { |
| 60 | struct div6_clock *clock = to_div6_clock(hw); |
Geert Uytterhoeven | 7980a86 | 2014-11-24 15:57:59 +0100 | [diff] [blame] | 61 | u32 val; |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 62 | |
Geert Uytterhoeven | c733c7d | 2018-03-15 10:43:12 +0100 | [diff] [blame] | 63 | val = readl(clock->reg); |
Geert Uytterhoeven | 7980a86 | 2014-11-24 15:57:59 +0100 | [diff] [blame] | 64 | val |= CPG_DIV6_CKSTP; |
| 65 | /* |
| 66 | * DIV6 clocks require the divisor field to be non-zero when stopping |
| 67 | * the clock. However, some clocks (e.g. ZB on sh73a0) fail to be |
| 68 | * re-enabled later if the divisor field is changed when stopping the |
| 69 | * clock |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 70 | */ |
Geert Uytterhoeven | 7980a86 | 2014-11-24 15:57:59 +0100 | [diff] [blame] | 71 | if (!(val & CPG_DIV6_DIV_MASK)) |
| 72 | val |= CPG_DIV6_DIV_MASK; |
Geert Uytterhoeven | c733c7d | 2018-03-15 10:43:12 +0100 | [diff] [blame] | 73 | writel(val, clock->reg); |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 74 | } |
| 75 | |
| 76 | static int cpg_div6_clock_is_enabled(struct clk_hw *hw) |
| 77 | { |
| 78 | struct div6_clock *clock = to_div6_clock(hw); |
| 79 | |
Geert Uytterhoeven | c733c7d | 2018-03-15 10:43:12 +0100 | [diff] [blame] | 80 | return !(readl(clock->reg) & CPG_DIV6_CKSTP); |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 81 | } |
| 82 | |
| 83 | static unsigned long cpg_div6_clock_recalc_rate(struct clk_hw *hw, |
| 84 | unsigned long parent_rate) |
| 85 | { |
| 86 | struct div6_clock *clock = to_div6_clock(hw); |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 87 | |
Geert Uytterhoeven | 3092d3b | 2016-02-18 15:16:02 +0100 | [diff] [blame] | 88 | return parent_rate / clock->div; |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 89 | } |
| 90 | |
| 91 | static unsigned int cpg_div6_clock_calc_div(unsigned long rate, |
| 92 | unsigned long parent_rate) |
| 93 | { |
| 94 | unsigned int div; |
| 95 | |
Geert Uytterhoeven | 5469d4f | 2015-02-04 13:27:21 +0100 | [diff] [blame] | 96 | if (!rate) |
| 97 | rate = 1; |
| 98 | |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 99 | div = DIV_ROUND_CLOSEST(parent_rate, rate); |
Geert Uytterhoeven | 6c7bc7db | 2021-04-01 15:01:34 +0200 | [diff] [blame] | 100 | return clamp(div, 1U, 64U); |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 101 | } |
| 102 | |
Geert Uytterhoeven | c9d1b58 | 2021-04-01 15:01:36 +0200 | [diff] [blame] | 103 | static int cpg_div6_clock_determine_rate(struct clk_hw *hw, |
| 104 | struct clk_rate_request *req) |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 105 | { |
Geert Uytterhoeven | 1c924fc | 2021-04-01 15:01:37 +0200 | [diff] [blame] | 106 | unsigned long prate, calc_rate, diff, best_rate, best_prate; |
| 107 | unsigned int num_parents = clk_hw_get_num_parents(hw); |
| 108 | struct clk_hw *parent, *best_parent = NULL; |
Geert Uytterhoeven | 02c6959 | 2021-04-01 15:01:38 +0200 | [diff] [blame] | 109 | unsigned int i, min_div, max_div, div; |
Geert Uytterhoeven | 1c924fc | 2021-04-01 15:01:37 +0200 | [diff] [blame] | 110 | unsigned long min_diff = ULONG_MAX; |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 111 | |
Geert Uytterhoeven | 1c924fc | 2021-04-01 15:01:37 +0200 | [diff] [blame] | 112 | for (i = 0; i < num_parents; i++) { |
| 113 | parent = clk_hw_get_parent_by_index(hw, i); |
| 114 | if (!parent) |
| 115 | continue; |
| 116 | |
| 117 | prate = clk_hw_get_rate(parent); |
| 118 | if (!prate) |
| 119 | continue; |
| 120 | |
Geert Uytterhoeven | 02c6959 | 2021-04-01 15:01:38 +0200 | [diff] [blame] | 121 | min_div = max(DIV_ROUND_UP(prate, req->max_rate), 1UL); |
| 122 | max_div = req->min_rate ? min(prate / req->min_rate, 64UL) : 64; |
| 123 | if (max_div < min_div) |
| 124 | continue; |
| 125 | |
Geert Uytterhoeven | 1c924fc | 2021-04-01 15:01:37 +0200 | [diff] [blame] | 126 | div = cpg_div6_clock_calc_div(req->rate, prate); |
Geert Uytterhoeven | 02c6959 | 2021-04-01 15:01:38 +0200 | [diff] [blame] | 127 | div = clamp(div, min_div, max_div); |
Geert Uytterhoeven | 1c924fc | 2021-04-01 15:01:37 +0200 | [diff] [blame] | 128 | calc_rate = prate / div; |
| 129 | diff = calc_rate > req->rate ? calc_rate - req->rate |
| 130 | : req->rate - calc_rate; |
| 131 | if (diff < min_diff) { |
| 132 | best_rate = calc_rate; |
| 133 | best_parent = parent; |
| 134 | best_prate = prate; |
| 135 | min_diff = diff; |
| 136 | } |
| 137 | } |
| 138 | |
| 139 | if (!best_parent) |
| 140 | return -EINVAL; |
| 141 | |
| 142 | req->best_parent_rate = best_prate; |
| 143 | req->best_parent_hw = best_parent; |
| 144 | req->rate = best_rate; |
Geert Uytterhoeven | c9d1b58 | 2021-04-01 15:01:36 +0200 | [diff] [blame] | 145 | return 0; |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 146 | } |
| 147 | |
| 148 | static int cpg_div6_clock_set_rate(struct clk_hw *hw, unsigned long rate, |
| 149 | unsigned long parent_rate) |
| 150 | { |
| 151 | struct div6_clock *clock = to_div6_clock(hw); |
| 152 | unsigned int div = cpg_div6_clock_calc_div(rate, parent_rate); |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 153 | u32 val; |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 154 | |
| 155 | clock->div = div; |
| 156 | |
Geert Uytterhoeven | c733c7d | 2018-03-15 10:43:12 +0100 | [diff] [blame] | 157 | val = readl(clock->reg) & ~CPG_DIV6_DIV_MASK; |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 158 | /* Only program the new divisor if the clock isn't stopped. */ |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 159 | if (!(val & CPG_DIV6_CKSTP)) |
Geert Uytterhoeven | c733c7d | 2018-03-15 10:43:12 +0100 | [diff] [blame] | 160 | writel(val | CPG_DIV6_DIV(clock->div - 1), clock->reg); |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 161 | |
| 162 | return 0; |
| 163 | } |
| 164 | |
| 165 | static u8 cpg_div6_clock_get_parent(struct clk_hw *hw) |
| 166 | { |
| 167 | struct div6_clock *clock = to_div6_clock(hw); |
| 168 | unsigned int i; |
| 169 | u8 hw_index; |
| 170 | |
Geert Uytterhoeven | 23b04c8 | 2021-04-01 15:01:35 +0200 | [diff] [blame] | 171 | if (clock->src_mask == 0) |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 172 | return 0; |
| 173 | |
Geert Uytterhoeven | 23b04c8 | 2021-04-01 15:01:35 +0200 | [diff] [blame] | 174 | hw_index = (readl(clock->reg) & clock->src_mask) >> |
| 175 | __ffs(clock->src_mask); |
Stephen Boyd | 497295a | 2015-06-25 16:53:23 -0700 | [diff] [blame] | 176 | for (i = 0; i < clk_hw_get_num_parents(hw); i++) { |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 177 | if (clock->parents[i] == hw_index) |
| 178 | return i; |
| 179 | } |
| 180 | |
| 181 | pr_err("%s: %s DIV6 clock set to invalid parent %u\n", |
Stephen Boyd | 836ee0f | 2015-08-12 11:42:23 -0700 | [diff] [blame] | 182 | __func__, clk_hw_get_name(hw), hw_index); |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 183 | return 0; |
| 184 | } |
| 185 | |
| 186 | static int cpg_div6_clock_set_parent(struct clk_hw *hw, u8 index) |
| 187 | { |
| 188 | struct div6_clock *clock = to_div6_clock(hw); |
Geert Uytterhoeven | 23b04c8 | 2021-04-01 15:01:35 +0200 | [diff] [blame] | 189 | u32 src; |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 190 | |
Stephen Boyd | 497295a | 2015-06-25 16:53:23 -0700 | [diff] [blame] | 191 | if (index >= clk_hw_get_num_parents(hw)) |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 192 | return -EINVAL; |
| 193 | |
Geert Uytterhoeven | 23b04c8 | 2021-04-01 15:01:35 +0200 | [diff] [blame] | 194 | src = clock->parents[index] << __ffs(clock->src_mask); |
| 195 | writel((readl(clock->reg) & ~clock->src_mask) | src, clock->reg); |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 196 | return 0; |
| 197 | } |
| 198 | |
| 199 | static const struct clk_ops cpg_div6_clock_ops = { |
| 200 | .enable = cpg_div6_clock_enable, |
| 201 | .disable = cpg_div6_clock_disable, |
| 202 | .is_enabled = cpg_div6_clock_is_enabled, |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 203 | .get_parent = cpg_div6_clock_get_parent, |
| 204 | .set_parent = cpg_div6_clock_set_parent, |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 205 | .recalc_rate = cpg_div6_clock_recalc_rate, |
Geert Uytterhoeven | c9d1b58 | 2021-04-01 15:01:36 +0200 | [diff] [blame] | 206 | .determine_rate = cpg_div6_clock_determine_rate, |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 207 | .set_rate = cpg_div6_clock_set_rate, |
| 208 | }; |
| 209 | |
Geert Uytterhoeven | 9f8c71e | 2017-06-21 22:34:33 +0200 | [diff] [blame] | 210 | static int cpg_div6_clock_notifier_call(struct notifier_block *nb, |
| 211 | unsigned long action, void *data) |
| 212 | { |
| 213 | struct div6_clock *clock = container_of(nb, struct div6_clock, nb); |
| 214 | |
| 215 | switch (action) { |
| 216 | case PM_EVENT_RESUME: |
| 217 | /* |
| 218 | * TODO: This does not yet support DIV6 clocks with multiple |
| 219 | * parents, as the parent selection bits are not restored. |
| 220 | * Fortunately so far such DIV6 clocks are found only on |
| 221 | * R/SH-Mobile SoCs, while the resume functionality is only |
| 222 | * needed on R-Car Gen3. |
| 223 | */ |
| 224 | if (__clk_get_enable_count(clock->hw.clk)) |
| 225 | cpg_div6_clock_enable(&clock->hw); |
| 226 | else |
| 227 | cpg_div6_clock_disable(&clock->hw); |
| 228 | return NOTIFY_OK; |
| 229 | } |
| 230 | |
| 231 | return NOTIFY_DONE; |
| 232 | } |
Geert Uytterhoeven | 1fae91ec9 | 2015-10-16 11:41:19 +0200 | [diff] [blame] | 233 | |
| 234 | /** |
| 235 | * cpg_div6_register - Register a DIV6 clock |
| 236 | * @name: Name of the DIV6 clock |
| 237 | * @num_parents: Number of parent clocks of the DIV6 clock (1, 4, or 8) |
| 238 | * @parent_names: Array containing the names of the parent clocks |
| 239 | * @reg: Mapped register used to control the DIV6 clock |
Geert Uytterhoeven | 9f8c71e | 2017-06-21 22:34:33 +0200 | [diff] [blame] | 240 | * @notifiers: Optional notifier chain to save/restore state for system resume |
Geert Uytterhoeven | 1fae91ec9 | 2015-10-16 11:41:19 +0200 | [diff] [blame] | 241 | */ |
| 242 | struct clk * __init cpg_div6_register(const char *name, |
| 243 | unsigned int num_parents, |
| 244 | const char **parent_names, |
Geert Uytterhoeven | 9f8c71e | 2017-06-21 22:34:33 +0200 | [diff] [blame] | 245 | void __iomem *reg, |
| 246 | struct raw_notifier_head *notifiers) |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 247 | { |
Geert Uytterhoeven | 1fae91ec9 | 2015-10-16 11:41:19 +0200 | [diff] [blame] | 248 | unsigned int valid_parents; |
Geert Uytterhoeven | f2fb4fe | 2021-03-26 11:54:34 +0100 | [diff] [blame] | 249 | struct clk_init_data init = {}; |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 250 | struct div6_clock *clock; |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 251 | struct clk *clk; |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 252 | unsigned int i; |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 253 | |
Geert Uytterhoeven | ba03861 | 2019-06-12 17:22:18 +0200 | [diff] [blame] | 254 | clock = kzalloc(struct_size(clock, parents, num_parents), GFP_KERNEL); |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 255 | if (!clock) |
Geert Uytterhoeven | 1fae91ec9 | 2015-10-16 11:41:19 +0200 | [diff] [blame] | 256 | return ERR_PTR(-ENOMEM); |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 257 | |
Geert Uytterhoeven | 1fae91ec9 | 2015-10-16 11:41:19 +0200 | [diff] [blame] | 258 | clock->reg = reg; |
| 259 | |
| 260 | /* |
| 261 | * Read the divisor. Disabling the clock overwrites the divisor, so we |
| 262 | * need to cache its value for the enable operation. |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 263 | */ |
Geert Uytterhoeven | c733c7d | 2018-03-15 10:43:12 +0100 | [diff] [blame] | 264 | clock->div = (readl(clock->reg) & CPG_DIV6_DIV_MASK) + 1; |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 265 | |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 266 | switch (num_parents) { |
| 267 | case 1: |
| 268 | /* fixed parent clock */ |
Geert Uytterhoeven | 23b04c8 | 2021-04-01 15:01:35 +0200 | [diff] [blame] | 269 | clock->src_mask = 0; |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 270 | break; |
| 271 | case 4: |
| 272 | /* clock with EXSRC bits 6-7 */ |
Geert Uytterhoeven | 23b04c8 | 2021-04-01 15:01:35 +0200 | [diff] [blame] | 273 | clock->src_mask = GENMASK(7, 6); |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 274 | break; |
| 275 | case 8: |
| 276 | /* VCLK with EXSRC bits 12-14 */ |
Geert Uytterhoeven | 23b04c8 | 2021-04-01 15:01:35 +0200 | [diff] [blame] | 277 | clock->src_mask = GENMASK(14, 12); |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 278 | break; |
| 279 | default: |
| 280 | pr_err("%s: invalid number of parents for DIV6 clock %s\n", |
Geert Uytterhoeven | 1fae91ec9 | 2015-10-16 11:41:19 +0200 | [diff] [blame] | 281 | __func__, name); |
| 282 | clk = ERR_PTR(-EINVAL); |
Geert Uytterhoeven | ba03861 | 2019-06-12 17:22:18 +0200 | [diff] [blame] | 283 | goto free_clock; |
Geert Uytterhoeven | 1fae91ec9 | 2015-10-16 11:41:19 +0200 | [diff] [blame] | 284 | } |
| 285 | |
| 286 | /* Filter out invalid parents */ |
| 287 | for (i = 0, valid_parents = 0; i < num_parents; i++) { |
| 288 | if (parent_names[i]) { |
| 289 | parent_names[valid_parents] = parent_names[i]; |
| 290 | clock->parents[valid_parents] = i; |
| 291 | valid_parents++; |
| 292 | } |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 293 | } |
| 294 | |
| 295 | /* Register the clock. */ |
Geert Uytterhoeven | 1fae91ec9 | 2015-10-16 11:41:19 +0200 | [diff] [blame] | 296 | init.name = name; |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 297 | init.ops = &cpg_div6_clock_ops; |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 298 | init.parent_names = parent_names; |
| 299 | init.num_parents = valid_parents; |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 300 | |
| 301 | clock->hw.init = &init; |
| 302 | |
| 303 | clk = clk_register(NULL, &clock->hw); |
Geert Uytterhoeven | 1fae91ec9 | 2015-10-16 11:41:19 +0200 | [diff] [blame] | 304 | if (IS_ERR(clk)) |
Geert Uytterhoeven | ba03861 | 2019-06-12 17:22:18 +0200 | [diff] [blame] | 305 | goto free_clock; |
Geert Uytterhoeven | 1fae91ec9 | 2015-10-16 11:41:19 +0200 | [diff] [blame] | 306 | |
Geert Uytterhoeven | 9f8c71e | 2017-06-21 22:34:33 +0200 | [diff] [blame] | 307 | if (notifiers) { |
| 308 | clock->nb.notifier_call = cpg_div6_clock_notifier_call; |
| 309 | raw_notifier_chain_register(notifiers, &clock->nb); |
| 310 | } |
| 311 | |
Geert Uytterhoeven | 1fae91ec9 | 2015-10-16 11:41:19 +0200 | [diff] [blame] | 312 | return clk; |
| 313 | |
Geert Uytterhoeven | 1fae91ec9 | 2015-10-16 11:41:19 +0200 | [diff] [blame] | 314 | free_clock: |
| 315 | kfree(clock); |
| 316 | return clk; |
| 317 | } |
| 318 | |
| 319 | static void __init cpg_div6_clock_init(struct device_node *np) |
| 320 | { |
| 321 | unsigned int num_parents; |
| 322 | const char **parent_names; |
| 323 | const char *clk_name = np->name; |
| 324 | void __iomem *reg; |
| 325 | struct clk *clk; |
| 326 | unsigned int i; |
| 327 | |
| 328 | num_parents = of_clk_get_parent_count(np); |
| 329 | if (num_parents < 1) { |
Rob Herring | e665f02 | 2018-08-28 10:44:29 -0500 | [diff] [blame] | 330 | pr_err("%s: no parent found for %pOFn DIV6 clock\n", |
| 331 | __func__, np); |
Geert Uytterhoeven | 1fae91ec9 | 2015-10-16 11:41:19 +0200 | [diff] [blame] | 332 | return; |
| 333 | } |
| 334 | |
| 335 | parent_names = kmalloc_array(num_parents, sizeof(*parent_names), |
| 336 | GFP_KERNEL); |
| 337 | if (!parent_names) |
| 338 | return; |
| 339 | |
| 340 | reg = of_iomap(np, 0); |
| 341 | if (reg == NULL) { |
Rob Herring | e665f02 | 2018-08-28 10:44:29 -0500 | [diff] [blame] | 342 | pr_err("%s: failed to map %pOFn DIV6 clock register\n", |
| 343 | __func__, np); |
Geert Uytterhoeven | 1fae91ec9 | 2015-10-16 11:41:19 +0200 | [diff] [blame] | 344 | goto error; |
| 345 | } |
| 346 | |
| 347 | /* Parse the DT properties. */ |
| 348 | of_property_read_string(np, "clock-output-names", &clk_name); |
| 349 | |
| 350 | for (i = 0; i < num_parents; i++) |
| 351 | parent_names[i] = of_clk_get_parent_name(np, i); |
| 352 | |
Geert Uytterhoeven | 9f8c71e | 2017-06-21 22:34:33 +0200 | [diff] [blame] | 353 | clk = cpg_div6_register(clk_name, num_parents, parent_names, reg, NULL); |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 354 | if (IS_ERR(clk)) { |
Rob Herring | e665f02 | 2018-08-28 10:44:29 -0500 | [diff] [blame] | 355 | pr_err("%s: failed to register %pOFn DIV6 clock (%ld)\n", |
| 356 | __func__, np, PTR_ERR(clk)); |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 357 | goto error; |
| 358 | } |
| 359 | |
| 360 | of_clk_add_provider(np, of_clk_src_simple_get, clk); |
| 361 | |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 362 | kfree(parent_names); |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 363 | return; |
| 364 | |
| 365 | error: |
Geert Uytterhoeven | 1fae91ec9 | 2015-10-16 11:41:19 +0200 | [diff] [blame] | 366 | if (reg) |
| 367 | iounmap(reg); |
Ulrich Hecht | c6d67fb | 2014-11-07 16:51:07 +0100 | [diff] [blame] | 368 | kfree(parent_names); |
Laurent Pinchart | abe844a | 2013-10-17 23:54:07 +0200 | [diff] [blame] | 369 | } |
| 370 | CLK_OF_DECLARE(cpg_div6_clk, "renesas,cpg-div6-clock", cpg_div6_clock_init); |