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 static u8 clk_composite_get_parent(struct clk_hw *hw) 23 { 24 struct clk_composite *composite = to_clk_composite(hw); 25 const struct clk_ops *mux_ops = composite->mux_ops; 26 struct clk_hw *mux_hw = composite->mux_hw; 27 28 __clk_hw_set_clk(mux_hw, hw); 29 30 return mux_ops->get_parent(mux_hw); 31 } 32 33 static int clk_composite_set_parent(struct clk_hw *hw, u8 index) 34 { 35 struct clk_composite *composite = to_clk_composite(hw); 36 const struct clk_ops *mux_ops = composite->mux_ops; 37 struct clk_hw *mux_hw = composite->mux_hw; 38 39 __clk_hw_set_clk(mux_hw, hw); 40 41 return mux_ops->set_parent(mux_hw, index); 42 } 43 44 static unsigned long clk_composite_recalc_rate(struct clk_hw *hw, 45 unsigned long parent_rate) 46 { 47 struct clk_composite *composite = to_clk_composite(hw); 48 const struct clk_ops *rate_ops = composite->rate_ops; 49 struct clk_hw *rate_hw = composite->rate_hw; 50 51 __clk_hw_set_clk(rate_hw, hw); 52 53 return rate_ops->recalc_rate(rate_hw, parent_rate); 54 } 55 56 static int clk_composite_determine_rate(struct clk_hw *hw, 57 struct clk_rate_request *req) 58 { 59 struct clk_composite *composite = to_clk_composite(hw); 60 const struct clk_ops *rate_ops = composite->rate_ops; 61 const struct clk_ops *mux_ops = composite->mux_ops; 62 struct clk_hw *rate_hw = composite->rate_hw; 63 struct clk_hw *mux_hw = composite->mux_hw; 64 struct clk_hw *parent; 65 unsigned long parent_rate; 66 long tmp_rate, best_rate = 0; 67 unsigned long rate_diff; 68 unsigned long best_rate_diff = ULONG_MAX; 69 long rate; 70 int i; 71 72 if (rate_hw && rate_ops && rate_ops->determine_rate) { 73 __clk_hw_set_clk(rate_hw, hw); 74 return rate_ops->determine_rate(rate_hw, req); 75 } else if (rate_hw && rate_ops && rate_ops->round_rate && 76 mux_hw && mux_ops && mux_ops->set_parent) { 77 req->best_parent_hw = NULL; 78 79 if (clk_hw_get_flags(hw) & CLK_SET_RATE_NO_REPARENT) { 80 parent = clk_hw_get_parent(mux_hw); 81 req->best_parent_hw = parent; 82 req->best_parent_rate = clk_hw_get_rate(parent); 83 84 rate = rate_ops->round_rate(rate_hw, req->rate, 85 &req->best_parent_rate); 86 if (rate < 0) 87 return rate; 88 89 req->rate = rate; 90 return 0; 91 } 92 93 for (i = 0; i < clk_hw_get_num_parents(mux_hw); i++) { 94 parent = clk_hw_get_parent_by_index(mux_hw, i); 95 if (!parent) 96 continue; 97 98 parent_rate = clk_hw_get_rate(parent); 99 100 tmp_rate = rate_ops->round_rate(rate_hw, req->rate, 101 &parent_rate); 102 if (tmp_rate < 0) 103 continue; 104 105 rate_diff = abs(req->rate - tmp_rate); 106 107 if (!rate_diff || !req->best_parent_hw 108 || best_rate_diff > rate_diff) { 109 req->best_parent_hw = parent; 110 req->best_parent_rate = parent_rate; 111 best_rate_diff = rate_diff; 112 best_rate = tmp_rate; 113 } 114 115 if (!rate_diff) 116 return 0; 117 } 118 119 req->rate = best_rate; 120 return 0; 121 } else if (mux_hw && mux_ops && mux_ops->determine_rate) { 122 __clk_hw_set_clk(mux_hw, hw); 123 return mux_ops->determine_rate(mux_hw, req); 124 } else { 125 pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n"); 126 return -EINVAL; 127 } 128 } 129 130 static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate, 131 unsigned long *prate) 132 { 133 struct clk_composite *composite = to_clk_composite(hw); 134 const struct clk_ops *rate_ops = composite->rate_ops; 135 struct clk_hw *rate_hw = composite->rate_hw; 136 137 __clk_hw_set_clk(rate_hw, hw); 138 139 return rate_ops->round_rate(rate_hw, rate, prate); 140 } 141 142 static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate, 143 unsigned long parent_rate) 144 { 145 struct clk_composite *composite = to_clk_composite(hw); 146 const struct clk_ops *rate_ops = composite->rate_ops; 147 struct clk_hw *rate_hw = composite->rate_hw; 148 149 __clk_hw_set_clk(rate_hw, hw); 150 151 return rate_ops->set_rate(rate_hw, rate, parent_rate); 152 } 153 154 static int clk_composite_set_rate_and_parent(struct clk_hw *hw, 155 unsigned long rate, 156 unsigned long parent_rate, 157 u8 index) 158 { 159 struct clk_composite *composite = to_clk_composite(hw); 160 const struct clk_ops *rate_ops = composite->rate_ops; 161 const struct clk_ops *mux_ops = composite->mux_ops; 162 struct clk_hw *rate_hw = composite->rate_hw; 163 struct clk_hw *mux_hw = composite->mux_hw; 164 unsigned long temp_rate; 165 166 __clk_hw_set_clk(rate_hw, hw); 167 __clk_hw_set_clk(mux_hw, hw); 168 169 temp_rate = rate_ops->recalc_rate(rate_hw, parent_rate); 170 if (temp_rate > rate) { 171 rate_ops->set_rate(rate_hw, rate, parent_rate); 172 mux_ops->set_parent(mux_hw, index); 173 } else { 174 mux_ops->set_parent(mux_hw, index); 175 rate_ops->set_rate(rate_hw, rate, parent_rate); 176 } 177 178 return 0; 179 } 180 181 static int clk_composite_is_enabled(struct clk_hw *hw) 182 { 183 struct clk_composite *composite = to_clk_composite(hw); 184 const struct clk_ops *gate_ops = composite->gate_ops; 185 struct clk_hw *gate_hw = composite->gate_hw; 186 187 __clk_hw_set_clk(gate_hw, hw); 188 189 return gate_ops->is_enabled(gate_hw); 190 } 191 192 static int clk_composite_enable(struct clk_hw *hw) 193 { 194 struct clk_composite *composite = to_clk_composite(hw); 195 const struct clk_ops *gate_ops = composite->gate_ops; 196 struct clk_hw *gate_hw = composite->gate_hw; 197 198 __clk_hw_set_clk(gate_hw, hw); 199 200 return gate_ops->enable(gate_hw); 201 } 202 203 static void clk_composite_disable(struct clk_hw *hw) 204 { 205 struct clk_composite *composite = to_clk_composite(hw); 206 const struct clk_ops *gate_ops = composite->gate_ops; 207 struct clk_hw *gate_hw = composite->gate_hw; 208 209 __clk_hw_set_clk(gate_hw, hw); 210 211 gate_ops->disable(gate_hw); 212 } 213 214 struct clk_hw *clk_hw_register_composite(struct device *dev, const char *name, 215 const char * const *parent_names, int num_parents, 216 struct clk_hw *mux_hw, const struct clk_ops *mux_ops, 217 struct clk_hw *rate_hw, const struct clk_ops *rate_ops, 218 struct clk_hw *gate_hw, const struct clk_ops *gate_ops, 219 unsigned long flags) 220 { 221 struct clk_hw *hw; 222 struct clk_init_data init; 223 struct clk_composite *composite; 224 struct clk_ops *clk_composite_ops; 225 int ret; 226 227 composite = kzalloc(sizeof(*composite), GFP_KERNEL); 228 if (!composite) 229 return ERR_PTR(-ENOMEM); 230 231 init.name = name; 232 init.flags = flags | CLK_IS_BASIC; 233 init.parent_names = parent_names; 234 init.num_parents = num_parents; 235 hw = &composite->hw; 236 237 clk_composite_ops = &composite->ops; 238 239 if (mux_hw && mux_ops) { 240 if (!mux_ops->get_parent) { 241 hw = ERR_PTR(-EINVAL); 242 goto err; 243 } 244 245 composite->mux_hw = mux_hw; 246 composite->mux_ops = mux_ops; 247 clk_composite_ops->get_parent = clk_composite_get_parent; 248 if (mux_ops->set_parent) 249 clk_composite_ops->set_parent = clk_composite_set_parent; 250 if (mux_ops->determine_rate) 251 clk_composite_ops->determine_rate = clk_composite_determine_rate; 252 } 253 254 if (rate_hw && rate_ops) { 255 if (!rate_ops->recalc_rate) { 256 hw = ERR_PTR(-EINVAL); 257 goto err; 258 } 259 clk_composite_ops->recalc_rate = clk_composite_recalc_rate; 260 261 if (rate_ops->determine_rate) 262 clk_composite_ops->determine_rate = 263 clk_composite_determine_rate; 264 else if (rate_ops->round_rate) 265 clk_composite_ops->round_rate = 266 clk_composite_round_rate; 267 268 /* .set_rate requires either .round_rate or .determine_rate */ 269 if (rate_ops->set_rate) { 270 if (rate_ops->determine_rate || rate_ops->round_rate) 271 clk_composite_ops->set_rate = 272 clk_composite_set_rate; 273 else 274 WARN(1, "%s: missing round_rate op is required\n", 275 __func__); 276 } 277 278 composite->rate_hw = rate_hw; 279 composite->rate_ops = rate_ops; 280 } 281 282 if (mux_hw && mux_ops && rate_hw && rate_ops) { 283 if (mux_ops->set_parent && rate_ops->set_rate) 284 clk_composite_ops->set_rate_and_parent = 285 clk_composite_set_rate_and_parent; 286 } 287 288 if (gate_hw && gate_ops) { 289 if (!gate_ops->is_enabled || !gate_ops->enable || 290 !gate_ops->disable) { 291 hw = ERR_PTR(-EINVAL); 292 goto err; 293 } 294 295 composite->gate_hw = gate_hw; 296 composite->gate_ops = gate_ops; 297 clk_composite_ops->is_enabled = clk_composite_is_enabled; 298 clk_composite_ops->enable = clk_composite_enable; 299 clk_composite_ops->disable = clk_composite_disable; 300 } 301 302 init.ops = clk_composite_ops; 303 composite->hw.init = &init; 304 305 ret = clk_hw_register(dev, hw); 306 if (ret) { 307 hw = ERR_PTR(ret); 308 goto err; 309 } 310 311 if (composite->mux_hw) 312 composite->mux_hw->clk = hw->clk; 313 314 if (composite->rate_hw) 315 composite->rate_hw->clk = hw->clk; 316 317 if (composite->gate_hw) 318 composite->gate_hw->clk = hw->clk; 319 320 return hw; 321 322 err: 323 kfree(composite); 324 return hw; 325 } 326 327 struct clk *clk_register_composite(struct device *dev, const char *name, 328 const char * const *parent_names, int num_parents, 329 struct clk_hw *mux_hw, const struct clk_ops *mux_ops, 330 struct clk_hw *rate_hw, const struct clk_ops *rate_ops, 331 struct clk_hw *gate_hw, const struct clk_ops *gate_ops, 332 unsigned long flags) 333 { 334 struct clk_hw *hw; 335 336 hw = clk_hw_register_composite(dev, name, parent_names, num_parents, 337 mux_hw, mux_ops, rate_hw, rate_ops, gate_hw, gate_ops, 338 flags); 339 if (IS_ERR(hw)) 340 return ERR_CAST(hw); 341 return hw->clk; 342 } 343 344 void clk_unregister_composite(struct clk *clk) 345 { 346 struct clk_composite *composite; 347 struct clk_hw *hw; 348 349 hw = __clk_get_hw(clk); 350 if (!hw) 351 return; 352 353 composite = to_clk_composite(hw); 354 355 clk_unregister(clk); 356 kfree(composite); 357 } 358