Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (c) 2013 NVIDIA CORPORATION. All rights reserved. |
| 3 | * |
| 4 | * This program is free software; you can redistribute it and/or modify it |
| 5 | * under the terms and conditions of the GNU General Public License, |
| 6 | * version 2, as published by the Free Software Foundation. |
| 7 | * |
| 8 | * This program is distributed in the hope it will be useful, but WITHOUT |
| 9 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or |
| 10 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for |
| 11 | * more details. |
| 12 | * |
| 13 | * You should have received a copy of the GNU General Public License |
| 14 | * along with this program. If not, see <http://www.gnu.org/licenses/>. |
| 15 | */ |
| 16 | |
| 17 | #include <linux/clk.h> |
| 18 | #include <linux/clk-provider.h> |
| 19 | #include <linux/err.h> |
| 20 | #include <linux/slab.h> |
| 21 | |
| 22 | #define to_clk_composite(_hw) container_of(_hw, struct clk_composite, hw) |
| 23 | |
| 24 | static u8 clk_composite_get_parent(struct clk_hw *hw) |
| 25 | { |
| 26 | struct clk_composite *composite = to_clk_composite(hw); |
| 27 | const struct clk_ops *mux_ops = composite->mux_ops; |
| 28 | struct clk_hw *mux_hw = composite->mux_hw; |
| 29 | |
Javier Martinez Canillas | 4e907ef | 2015-02-12 14:58:30 +0100 | [diff] [blame^] | 30 | __clk_hw_set_clk(mux_hw, hw); |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 31 | |
| 32 | return mux_ops->get_parent(mux_hw); |
| 33 | } |
| 34 | |
| 35 | static int clk_composite_set_parent(struct clk_hw *hw, u8 index) |
| 36 | { |
| 37 | struct clk_composite *composite = to_clk_composite(hw); |
| 38 | const struct clk_ops *mux_ops = composite->mux_ops; |
| 39 | struct clk_hw *mux_hw = composite->mux_hw; |
| 40 | |
Javier Martinez Canillas | 4e907ef | 2015-02-12 14:58:30 +0100 | [diff] [blame^] | 41 | __clk_hw_set_clk(mux_hw, hw); |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 42 | |
| 43 | return mux_ops->set_parent(mux_hw, index); |
| 44 | } |
| 45 | |
| 46 | static unsigned long clk_composite_recalc_rate(struct clk_hw *hw, |
| 47 | unsigned long parent_rate) |
| 48 | { |
| 49 | struct clk_composite *composite = to_clk_composite(hw); |
Mike Turquette | d3a1c7b | 2013-04-11 11:31:36 -0700 | [diff] [blame] | 50 | const struct clk_ops *rate_ops = composite->rate_ops; |
| 51 | struct clk_hw *rate_hw = composite->rate_hw; |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 52 | |
Javier Martinez Canillas | 4e907ef | 2015-02-12 14:58:30 +0100 | [diff] [blame^] | 53 | __clk_hw_set_clk(rate_hw, hw); |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 54 | |
Mike Turquette | d3a1c7b | 2013-04-11 11:31:36 -0700 | [diff] [blame] | 55 | return rate_ops->recalc_rate(rate_hw, parent_rate); |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 56 | } |
| 57 | |
Emilio López | 107f319 | 2013-09-14 21:37:59 -0300 | [diff] [blame] | 58 | static long clk_composite_determine_rate(struct clk_hw *hw, unsigned long rate, |
Tomeu Vizoso | 1c8e600 | 2015-01-23 12:03:31 +0100 | [diff] [blame] | 59 | unsigned long min_rate, |
| 60 | unsigned long max_rate, |
Emilio López | 107f319 | 2013-09-14 21:37:59 -0300 | [diff] [blame] | 61 | unsigned long *best_parent_rate, |
Tomeu Vizoso | 646cafc | 2014-12-02 08:54:22 +0100 | [diff] [blame] | 62 | struct clk_hw **best_parent_p) |
Emilio López | 107f319 | 2013-09-14 21:37:59 -0300 | [diff] [blame] | 63 | { |
| 64 | struct clk_composite *composite = to_clk_composite(hw); |
| 65 | const struct clk_ops *rate_ops = composite->rate_ops; |
| 66 | const struct clk_ops *mux_ops = composite->mux_ops; |
| 67 | struct clk_hw *rate_hw = composite->rate_hw; |
| 68 | struct clk_hw *mux_hw = composite->mux_hw; |
Boris BREZILLON | 3eb635f | 2014-07-03 01:56:45 +0200 | [diff] [blame] | 69 | struct clk *parent; |
| 70 | unsigned long parent_rate; |
| 71 | long tmp_rate, best_rate = 0; |
| 72 | unsigned long rate_diff; |
| 73 | unsigned long best_rate_diff = ULONG_MAX; |
| 74 | int i; |
Emilio López | 107f319 | 2013-09-14 21:37:59 -0300 | [diff] [blame] | 75 | |
| 76 | if (rate_hw && rate_ops && rate_ops->determine_rate) { |
Javier Martinez Canillas | 4e907ef | 2015-02-12 14:58:30 +0100 | [diff] [blame^] | 77 | __clk_hw_set_clk(rate_hw, hw); |
Tomeu Vizoso | 1c8e600 | 2015-01-23 12:03:31 +0100 | [diff] [blame] | 78 | return rate_ops->determine_rate(rate_hw, rate, min_rate, |
| 79 | max_rate, |
| 80 | best_parent_rate, |
Emilio López | 107f319 | 2013-09-14 21:37:59 -0300 | [diff] [blame] | 81 | best_parent_p); |
Boris BREZILLON | 3eb635f | 2014-07-03 01:56:45 +0200 | [diff] [blame] | 82 | } else if (rate_hw && rate_ops && rate_ops->round_rate && |
| 83 | mux_hw && mux_ops && mux_ops->set_parent) { |
| 84 | *best_parent_p = NULL; |
| 85 | |
| 86 | if (__clk_get_flags(hw->clk) & CLK_SET_RATE_NO_REPARENT) { |
Tomeu Vizoso | 646cafc | 2014-12-02 08:54:22 +0100 | [diff] [blame] | 87 | parent = clk_get_parent(mux_hw->clk); |
| 88 | *best_parent_p = __clk_get_hw(parent); |
| 89 | *best_parent_rate = __clk_get_rate(parent); |
Boris BREZILLON | 3eb635f | 2014-07-03 01:56:45 +0200 | [diff] [blame] | 90 | |
| 91 | return rate_ops->round_rate(rate_hw, rate, |
| 92 | best_parent_rate); |
| 93 | } |
| 94 | |
| 95 | for (i = 0; i < __clk_get_num_parents(mux_hw->clk); i++) { |
| 96 | parent = clk_get_parent_by_index(mux_hw->clk, i); |
| 97 | if (!parent) |
| 98 | continue; |
| 99 | |
| 100 | parent_rate = __clk_get_rate(parent); |
| 101 | |
| 102 | tmp_rate = rate_ops->round_rate(rate_hw, rate, |
| 103 | &parent_rate); |
| 104 | if (tmp_rate < 0) |
| 105 | continue; |
| 106 | |
| 107 | rate_diff = abs(rate - tmp_rate); |
| 108 | |
| 109 | if (!rate_diff || !*best_parent_p |
| 110 | || best_rate_diff > rate_diff) { |
Tomeu Vizoso | 646cafc | 2014-12-02 08:54:22 +0100 | [diff] [blame] | 111 | *best_parent_p = __clk_get_hw(parent); |
Boris BREZILLON | 3eb635f | 2014-07-03 01:56:45 +0200 | [diff] [blame] | 112 | *best_parent_rate = parent_rate; |
| 113 | best_rate_diff = rate_diff; |
| 114 | best_rate = tmp_rate; |
| 115 | } |
| 116 | |
| 117 | if (!rate_diff) |
| 118 | return rate; |
| 119 | } |
| 120 | |
| 121 | return best_rate; |
Emilio López | 107f319 | 2013-09-14 21:37:59 -0300 | [diff] [blame] | 122 | } else if (mux_hw && mux_ops && mux_ops->determine_rate) { |
Javier Martinez Canillas | 4e907ef | 2015-02-12 14:58:30 +0100 | [diff] [blame^] | 123 | __clk_hw_set_clk(mux_hw, hw); |
Tomeu Vizoso | 1c8e600 | 2015-01-23 12:03:31 +0100 | [diff] [blame] | 124 | return mux_ops->determine_rate(mux_hw, rate, min_rate, |
| 125 | max_rate, best_parent_rate, |
Emilio López | 107f319 | 2013-09-14 21:37:59 -0300 | [diff] [blame] | 126 | best_parent_p); |
| 127 | } else { |
| 128 | pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n"); |
| 129 | return 0; |
| 130 | } |
| 131 | } |
| 132 | |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 133 | static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate, |
| 134 | unsigned long *prate) |
| 135 | { |
| 136 | struct clk_composite *composite = to_clk_composite(hw); |
Mike Turquette | d3a1c7b | 2013-04-11 11:31:36 -0700 | [diff] [blame] | 137 | const struct clk_ops *rate_ops = composite->rate_ops; |
| 138 | struct clk_hw *rate_hw = composite->rate_hw; |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 139 | |
Javier Martinez Canillas | 4e907ef | 2015-02-12 14:58:30 +0100 | [diff] [blame^] | 140 | __clk_hw_set_clk(rate_hw, hw); |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 141 | |
Mike Turquette | d3a1c7b | 2013-04-11 11:31:36 -0700 | [diff] [blame] | 142 | return rate_ops->round_rate(rate_hw, rate, prate); |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 143 | } |
| 144 | |
| 145 | static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate, |
| 146 | unsigned long parent_rate) |
| 147 | { |
| 148 | struct clk_composite *composite = to_clk_composite(hw); |
Mike Turquette | d3a1c7b | 2013-04-11 11:31:36 -0700 | [diff] [blame] | 149 | const struct clk_ops *rate_ops = composite->rate_ops; |
| 150 | struct clk_hw *rate_hw = composite->rate_hw; |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 151 | |
Javier Martinez Canillas | 4e907ef | 2015-02-12 14:58:30 +0100 | [diff] [blame^] | 152 | __clk_hw_set_clk(rate_hw, hw); |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 153 | |
Mike Turquette | d3a1c7b | 2013-04-11 11:31:36 -0700 | [diff] [blame] | 154 | return rate_ops->set_rate(rate_hw, rate, parent_rate); |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 155 | } |
| 156 | |
| 157 | static int clk_composite_is_enabled(struct clk_hw *hw) |
| 158 | { |
| 159 | struct clk_composite *composite = to_clk_composite(hw); |
| 160 | const struct clk_ops *gate_ops = composite->gate_ops; |
| 161 | struct clk_hw *gate_hw = composite->gate_hw; |
| 162 | |
Javier Martinez Canillas | 4e907ef | 2015-02-12 14:58:30 +0100 | [diff] [blame^] | 163 | __clk_hw_set_clk(gate_hw, hw); |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 164 | |
| 165 | return gate_ops->is_enabled(gate_hw); |
| 166 | } |
| 167 | |
| 168 | static int clk_composite_enable(struct clk_hw *hw) |
| 169 | { |
| 170 | struct clk_composite *composite = to_clk_composite(hw); |
| 171 | const struct clk_ops *gate_ops = composite->gate_ops; |
| 172 | struct clk_hw *gate_hw = composite->gate_hw; |
| 173 | |
Javier Martinez Canillas | 4e907ef | 2015-02-12 14:58:30 +0100 | [diff] [blame^] | 174 | __clk_hw_set_clk(gate_hw, hw); |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 175 | |
| 176 | return gate_ops->enable(gate_hw); |
| 177 | } |
| 178 | |
| 179 | static void clk_composite_disable(struct clk_hw *hw) |
| 180 | { |
| 181 | struct clk_composite *composite = to_clk_composite(hw); |
| 182 | const struct clk_ops *gate_ops = composite->gate_ops; |
| 183 | struct clk_hw *gate_hw = composite->gate_hw; |
| 184 | |
Javier Martinez Canillas | 4e907ef | 2015-02-12 14:58:30 +0100 | [diff] [blame^] | 185 | __clk_hw_set_clk(gate_hw, hw); |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 186 | |
| 187 | gate_ops->disable(gate_hw); |
| 188 | } |
| 189 | |
| 190 | struct clk *clk_register_composite(struct device *dev, const char *name, |
| 191 | const char **parent_names, int num_parents, |
| 192 | struct clk_hw *mux_hw, const struct clk_ops *mux_ops, |
Mike Turquette | d3a1c7b | 2013-04-11 11:31:36 -0700 | [diff] [blame] | 193 | struct clk_hw *rate_hw, const struct clk_ops *rate_ops, |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 194 | struct clk_hw *gate_hw, const struct clk_ops *gate_ops, |
| 195 | unsigned long flags) |
| 196 | { |
| 197 | struct clk *clk; |
| 198 | struct clk_init_data init; |
| 199 | struct clk_composite *composite; |
| 200 | struct clk_ops *clk_composite_ops; |
| 201 | |
| 202 | composite = kzalloc(sizeof(*composite), GFP_KERNEL); |
| 203 | if (!composite) { |
| 204 | pr_err("%s: could not allocate composite clk\n", __func__); |
| 205 | return ERR_PTR(-ENOMEM); |
| 206 | } |
| 207 | |
| 208 | init.name = name; |
| 209 | init.flags = flags | CLK_IS_BASIC; |
| 210 | init.parent_names = parent_names; |
| 211 | init.num_parents = num_parents; |
| 212 | |
| 213 | clk_composite_ops = &composite->ops; |
| 214 | |
| 215 | if (mux_hw && mux_ops) { |
Heiko Stübner | 0c02cf2 | 2014-07-03 01:57:30 +0200 | [diff] [blame] | 216 | if (!mux_ops->get_parent) { |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 217 | clk = ERR_PTR(-EINVAL); |
| 218 | goto err; |
| 219 | } |
| 220 | |
| 221 | composite->mux_hw = mux_hw; |
| 222 | composite->mux_ops = mux_ops; |
| 223 | clk_composite_ops->get_parent = clk_composite_get_parent; |
Heiko Stübner | 0c02cf2 | 2014-07-03 01:57:30 +0200 | [diff] [blame] | 224 | if (mux_ops->set_parent) |
| 225 | clk_composite_ops->set_parent = clk_composite_set_parent; |
Emilio López | 107f319 | 2013-09-14 21:37:59 -0300 | [diff] [blame] | 226 | if (mux_ops->determine_rate) |
| 227 | clk_composite_ops->determine_rate = clk_composite_determine_rate; |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 228 | } |
| 229 | |
Mike Turquette | d3a1c7b | 2013-04-11 11:31:36 -0700 | [diff] [blame] | 230 | if (rate_hw && rate_ops) { |
Mike Turquette | f363e21 | 2013-04-11 11:31:37 -0700 | [diff] [blame] | 231 | if (!rate_ops->recalc_rate) { |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 232 | clk = ERR_PTR(-EINVAL); |
| 233 | goto err; |
| 234 | } |
Mike Turquette | 5a994e1 | 2014-07-03 01:58:14 +0200 | [diff] [blame] | 235 | clk_composite_ops->recalc_rate = clk_composite_recalc_rate; |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 236 | |
Mike Turquette | 5a994e1 | 2014-07-03 01:58:14 +0200 | [diff] [blame] | 237 | if (rate_ops->determine_rate) |
| 238 | clk_composite_ops->determine_rate = |
| 239 | clk_composite_determine_rate; |
| 240 | else if (rate_ops->round_rate) |
| 241 | clk_composite_ops->round_rate = |
| 242 | clk_composite_round_rate; |
| 243 | |
| 244 | /* .set_rate requires either .round_rate or .determine_rate */ |
| 245 | if (rate_ops->set_rate) { |
| 246 | if (rate_ops->determine_rate || rate_ops->round_rate) |
| 247 | clk_composite_ops->set_rate = |
| 248 | clk_composite_set_rate; |
| 249 | else |
| 250 | WARN(1, "%s: missing round_rate op is required\n", |
| 251 | __func__); |
Mike Turquette | f363e21 | 2013-04-11 11:31:37 -0700 | [diff] [blame] | 252 | } |
| 253 | |
Mike Turquette | d3a1c7b | 2013-04-11 11:31:36 -0700 | [diff] [blame] | 254 | composite->rate_hw = rate_hw; |
| 255 | composite->rate_ops = rate_ops; |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 256 | } |
| 257 | |
| 258 | if (gate_hw && gate_ops) { |
| 259 | if (!gate_ops->is_enabled || !gate_ops->enable || |
| 260 | !gate_ops->disable) { |
| 261 | clk = ERR_PTR(-EINVAL); |
| 262 | goto err; |
| 263 | } |
| 264 | |
| 265 | composite->gate_hw = gate_hw; |
| 266 | composite->gate_ops = gate_ops; |
| 267 | clk_composite_ops->is_enabled = clk_composite_is_enabled; |
| 268 | clk_composite_ops->enable = clk_composite_enable; |
| 269 | clk_composite_ops->disable = clk_composite_disable; |
| 270 | } |
| 271 | |
| 272 | init.ops = clk_composite_ops; |
| 273 | composite->hw.init = &init; |
| 274 | |
| 275 | clk = clk_register(dev, &composite->hw); |
| 276 | if (IS_ERR(clk)) |
| 277 | goto err; |
| 278 | |
| 279 | if (composite->mux_hw) |
| 280 | composite->mux_hw->clk = clk; |
| 281 | |
Mike Turquette | d3a1c7b | 2013-04-11 11:31:36 -0700 | [diff] [blame] | 282 | if (composite->rate_hw) |
| 283 | composite->rate_hw->clk = clk; |
Prashant Gaikwad | ece7009 | 2013-03-20 17:30:34 +0530 | [diff] [blame] | 284 | |
| 285 | if (composite->gate_hw) |
| 286 | composite->gate_hw->clk = clk; |
| 287 | |
| 288 | return clk; |
| 289 | |
| 290 | err: |
| 291 | kfree(composite); |
| 292 | return clk; |
| 293 | } |