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 30 mux_hw->clk = hw->clk; 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 41 mux_hw->clk = hw->clk; 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); 50 const struct clk_ops *rate_ops = composite->rate_ops; 51 struct clk_hw *rate_hw = composite->rate_hw; 52 53 rate_hw->clk = hw->clk; 54 55 return rate_ops->recalc_rate(rate_hw, parent_rate); 56 } 57 58 static long clk_composite_determine_rate(struct clk_hw *hw, unsigned long rate, 59 unsigned long *best_parent_rate, 60 struct clk **best_parent_p) 61 { 62 struct clk_composite *composite = to_clk_composite(hw); 63 const struct clk_ops *rate_ops = composite->rate_ops; 64 const struct clk_ops *mux_ops = composite->mux_ops; 65 struct clk_hw *rate_hw = composite->rate_hw; 66 struct clk_hw *mux_hw = composite->mux_hw; 67 struct clk *parent; 68 unsigned long parent_rate; 69 long tmp_rate, best_rate = 0; 70 unsigned long rate_diff; 71 unsigned long best_rate_diff = ULONG_MAX; 72 int i; 73 74 if (rate_hw && rate_ops && rate_ops->determine_rate) { 75 rate_hw->clk = hw->clk; 76 return rate_ops->determine_rate(rate_hw, rate, best_parent_rate, 77 best_parent_p); 78 } else if (rate_hw && rate_ops && rate_ops->round_rate && 79 mux_hw && mux_ops && mux_ops->set_parent) { 80 *best_parent_p = NULL; 81 82 if (__clk_get_flags(hw->clk) & CLK_SET_RATE_NO_REPARENT) { 83 *best_parent_p = clk_get_parent(mux_hw->clk); 84 *best_parent_rate = __clk_get_rate(*best_parent_p); 85 86 return rate_ops->round_rate(rate_hw, rate, 87 best_parent_rate); 88 } 89 90 for (i = 0; i < __clk_get_num_parents(mux_hw->clk); i++) { 91 parent = clk_get_parent_by_index(mux_hw->clk, i); 92 if (!parent) 93 continue; 94 95 parent_rate = __clk_get_rate(parent); 96 97 tmp_rate = rate_ops->round_rate(rate_hw, rate, 98 &parent_rate); 99 if (tmp_rate < 0) 100 continue; 101 102 rate_diff = abs(rate - tmp_rate); 103 104 if (!rate_diff || !*best_parent_p 105 || best_rate_diff > rate_diff) { 106 *best_parent_p = parent; 107 *best_parent_rate = parent_rate; 108 best_rate_diff = rate_diff; 109 best_rate = tmp_rate; 110 } 111 112 if (!rate_diff) 113 return rate; 114 } 115 116 return best_rate; 117 } else if (mux_hw && mux_ops && mux_ops->determine_rate) { 118 mux_hw->clk = hw->clk; 119 return mux_ops->determine_rate(mux_hw, rate, best_parent_rate, 120 best_parent_p); 121 } else { 122 pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n"); 123 return 0; 124 } 125 } 126 127 static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate, 128 unsigned long *prate) 129 { 130 struct clk_composite *composite = to_clk_composite(hw); 131 const struct clk_ops *rate_ops = composite->rate_ops; 132 struct clk_hw *rate_hw = composite->rate_hw; 133 134 rate_hw->clk = hw->clk; 135 136 return rate_ops->round_rate(rate_hw, rate, prate); 137 } 138 139 static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate, 140 unsigned long parent_rate) 141 { 142 struct clk_composite *composite = to_clk_composite(hw); 143 const struct clk_ops *rate_ops = composite->rate_ops; 144 struct clk_hw *rate_hw = composite->rate_hw; 145 146 rate_hw->clk = hw->clk; 147 148 return rate_ops->set_rate(rate_hw, rate, parent_rate); 149 } 150 151 static int clk_composite_is_enabled(struct clk_hw *hw) 152 { 153 struct clk_composite *composite = to_clk_composite(hw); 154 const struct clk_ops *gate_ops = composite->gate_ops; 155 struct clk_hw *gate_hw = composite->gate_hw; 156 157 gate_hw->clk = hw->clk; 158 159 return gate_ops->is_enabled(gate_hw); 160 } 161 162 static int clk_composite_enable(struct clk_hw *hw) 163 { 164 struct clk_composite *composite = to_clk_composite(hw); 165 const struct clk_ops *gate_ops = composite->gate_ops; 166 struct clk_hw *gate_hw = composite->gate_hw; 167 168 gate_hw->clk = hw->clk; 169 170 return gate_ops->enable(gate_hw); 171 } 172 173 static void clk_composite_disable(struct clk_hw *hw) 174 { 175 struct clk_composite *composite = to_clk_composite(hw); 176 const struct clk_ops *gate_ops = composite->gate_ops; 177 struct clk_hw *gate_hw = composite->gate_hw; 178 179 gate_hw->clk = hw->clk; 180 181 gate_ops->disable(gate_hw); 182 } 183 184 struct clk *clk_register_composite(struct device *dev, const char *name, 185 const char **parent_names, int num_parents, 186 struct clk_hw *mux_hw, const struct clk_ops *mux_ops, 187 struct clk_hw *rate_hw, const struct clk_ops *rate_ops, 188 struct clk_hw *gate_hw, const struct clk_ops *gate_ops, 189 unsigned long flags) 190 { 191 struct clk *clk; 192 struct clk_init_data init; 193 struct clk_composite *composite; 194 struct clk_ops *clk_composite_ops; 195 196 composite = kzalloc(sizeof(*composite), GFP_KERNEL); 197 if (!composite) { 198 pr_err("%s: could not allocate composite clk\n", __func__); 199 return ERR_PTR(-ENOMEM); 200 } 201 202 init.name = name; 203 init.flags = flags | CLK_IS_BASIC; 204 init.parent_names = parent_names; 205 init.num_parents = num_parents; 206 207 clk_composite_ops = &composite->ops; 208 209 if (mux_hw && mux_ops) { 210 if (!mux_ops->get_parent) { 211 clk = ERR_PTR(-EINVAL); 212 goto err; 213 } 214 215 composite->mux_hw = mux_hw; 216 composite->mux_ops = mux_ops; 217 clk_composite_ops->get_parent = clk_composite_get_parent; 218 if (mux_ops->set_parent) 219 clk_composite_ops->set_parent = clk_composite_set_parent; 220 if (mux_ops->determine_rate) 221 clk_composite_ops->determine_rate = clk_composite_determine_rate; 222 } 223 224 if (rate_hw && rate_ops) { 225 if (!rate_ops->recalc_rate) { 226 clk = ERR_PTR(-EINVAL); 227 goto err; 228 } 229 clk_composite_ops->recalc_rate = clk_composite_recalc_rate; 230 231 if (rate_ops->determine_rate) 232 clk_composite_ops->determine_rate = 233 clk_composite_determine_rate; 234 else if (rate_ops->round_rate) 235 clk_composite_ops->round_rate = 236 clk_composite_round_rate; 237 238 /* .set_rate requires either .round_rate or .determine_rate */ 239 if (rate_ops->set_rate) { 240 if (rate_ops->determine_rate || rate_ops->round_rate) 241 clk_composite_ops->set_rate = 242 clk_composite_set_rate; 243 else 244 WARN(1, "%s: missing round_rate op is required\n", 245 __func__); 246 } 247 248 composite->rate_hw = rate_hw; 249 composite->rate_ops = rate_ops; 250 } 251 252 if (gate_hw && gate_ops) { 253 if (!gate_ops->is_enabled || !gate_ops->enable || 254 !gate_ops->disable) { 255 clk = ERR_PTR(-EINVAL); 256 goto err; 257 } 258 259 composite->gate_hw = gate_hw; 260 composite->gate_ops = gate_ops; 261 clk_composite_ops->is_enabled = clk_composite_is_enabled; 262 clk_composite_ops->enable = clk_composite_enable; 263 clk_composite_ops->disable = clk_composite_disable; 264 } 265 266 init.ops = clk_composite_ops; 267 composite->hw.init = &init; 268 269 clk = clk_register(dev, &composite->hw); 270 if (IS_ERR(clk)) 271 goto err; 272 273 if (composite->mux_hw) 274 composite->mux_hw->clk = clk; 275 276 if (composite->rate_hw) 277 composite->rate_hw->clk = clk; 278 279 if (composite->gate_hw) 280 composite->gate_hw->clk = clk; 281 282 return clk; 283 284 err: 285 kfree(composite); 286 return clk; 287 } 288