493 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Copyright (c) 2013 NVIDIA CORPORATION. All rights reserved.4 */5 6#include <linux/clk-provider.h>7#include <linux/device.h>8#include <linux/err.h>9#include <linux/slab.h>10 11static u8 clk_composite_get_parent(struct clk_hw *hw)12{13 struct clk_composite *composite = to_clk_composite(hw);14 const struct clk_ops *mux_ops = composite->mux_ops;15 struct clk_hw *mux_hw = composite->mux_hw;16 17 __clk_hw_set_clk(mux_hw, hw);18 19 return mux_ops->get_parent(mux_hw);20}21 22static int clk_composite_set_parent(struct clk_hw *hw, u8 index)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->set_parent(mux_hw, index);31}32 33static unsigned long clk_composite_recalc_rate(struct clk_hw *hw,34 unsigned long parent_rate)35{36 struct clk_composite *composite = to_clk_composite(hw);37 const struct clk_ops *rate_ops = composite->rate_ops;38 struct clk_hw *rate_hw = composite->rate_hw;39 40 __clk_hw_set_clk(rate_hw, hw);41 42 return rate_ops->recalc_rate(rate_hw, parent_rate);43}44 45static int clk_composite_determine_rate_for_parent(struct clk_hw *rate_hw,46 struct clk_rate_request *req,47 struct clk_hw *parent_hw,48 const struct clk_ops *rate_ops)49{50 long rate;51 52 req->best_parent_hw = parent_hw;53 req->best_parent_rate = clk_hw_get_rate(parent_hw);54 55 if (rate_ops->determine_rate)56 return rate_ops->determine_rate(rate_hw, req);57 58 rate = rate_ops->round_rate(rate_hw, req->rate,59 &req->best_parent_rate);60 if (rate < 0)61 return rate;62 63 req->rate = rate;64 65 return 0;66}67 68static int clk_composite_determine_rate(struct clk_hw *hw,69 struct clk_rate_request *req)70{71 struct clk_composite *composite = to_clk_composite(hw);72 const struct clk_ops *rate_ops = composite->rate_ops;73 const struct clk_ops *mux_ops = composite->mux_ops;74 struct clk_hw *rate_hw = composite->rate_hw;75 struct clk_hw *mux_hw = composite->mux_hw;76 struct clk_hw *parent;77 unsigned long rate_diff;78 unsigned long best_rate_diff = ULONG_MAX;79 unsigned long best_rate = 0;80 int i, ret;81 82 if (rate_hw && rate_ops &&83 (rate_ops->determine_rate || rate_ops->round_rate) &&84 mux_hw && mux_ops && mux_ops->set_parent) {85 req->best_parent_hw = NULL;86 87 if (clk_hw_get_flags(hw) & CLK_SET_RATE_NO_REPARENT) {88 struct clk_rate_request tmp_req;89 90 parent = clk_hw_get_parent(mux_hw);91 92 clk_hw_forward_rate_request(hw, req, parent, &tmp_req, req->rate);93 ret = clk_composite_determine_rate_for_parent(rate_hw,94 &tmp_req,95 parent,96 rate_ops);97 if (ret)98 return ret;99 100 req->rate = tmp_req.rate;101 req->best_parent_hw = tmp_req.best_parent_hw;102 req->best_parent_rate = tmp_req.best_parent_rate;103 104 return 0;105 }106 107 for (i = 0; i < clk_hw_get_num_parents(mux_hw); i++) {108 struct clk_rate_request tmp_req;109 110 parent = clk_hw_get_parent_by_index(mux_hw, i);111 if (!parent)112 continue;113 114 clk_hw_forward_rate_request(hw, req, parent, &tmp_req, req->rate);115 ret = clk_composite_determine_rate_for_parent(rate_hw,116 &tmp_req,117 parent,118 rate_ops);119 if (ret)120 continue;121 122 if (req->rate >= tmp_req.rate)123 rate_diff = req->rate - tmp_req.rate;124 else125 rate_diff = tmp_req.rate - req->rate;126 127 if (!rate_diff || !req->best_parent_hw128 || best_rate_diff > rate_diff) {129 req->best_parent_hw = parent;130 req->best_parent_rate = tmp_req.best_parent_rate;131 best_rate_diff = rate_diff;132 best_rate = tmp_req.rate;133 }134 135 if (!rate_diff)136 return 0;137 }138 139 req->rate = best_rate;140 return 0;141 } else if (rate_hw && rate_ops && rate_ops->determine_rate) {142 __clk_hw_set_clk(rate_hw, hw);143 return rate_ops->determine_rate(rate_hw, req);144 } else if (mux_hw && mux_ops && mux_ops->determine_rate) {145 __clk_hw_set_clk(mux_hw, hw);146 return mux_ops->determine_rate(mux_hw, req);147 } else {148 pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n");149 return -EINVAL;150 }151}152 153static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate,154 unsigned long *prate)155{156 struct clk_composite *composite = to_clk_composite(hw);157 const struct clk_ops *rate_ops = composite->rate_ops;158 struct clk_hw *rate_hw = composite->rate_hw;159 160 __clk_hw_set_clk(rate_hw, hw);161 162 return rate_ops->round_rate(rate_hw, rate, prate);163}164 165static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate,166 unsigned long parent_rate)167{168 struct clk_composite *composite = to_clk_composite(hw);169 const struct clk_ops *rate_ops = composite->rate_ops;170 struct clk_hw *rate_hw = composite->rate_hw;171 172 __clk_hw_set_clk(rate_hw, hw);173 174 return rate_ops->set_rate(rate_hw, rate, parent_rate);175}176 177static int clk_composite_set_rate_and_parent(struct clk_hw *hw,178 unsigned long rate,179 unsigned long parent_rate,180 u8 index)181{182 struct clk_composite *composite = to_clk_composite(hw);183 const struct clk_ops *rate_ops = composite->rate_ops;184 const struct clk_ops *mux_ops = composite->mux_ops;185 struct clk_hw *rate_hw = composite->rate_hw;186 struct clk_hw *mux_hw = composite->mux_hw;187 unsigned long temp_rate;188 189 __clk_hw_set_clk(rate_hw, hw);190 __clk_hw_set_clk(mux_hw, hw);191 192 temp_rate = rate_ops->recalc_rate(rate_hw, parent_rate);193 if (temp_rate > rate) {194 rate_ops->set_rate(rate_hw, rate, parent_rate);195 mux_ops->set_parent(mux_hw, index);196 } else {197 mux_ops->set_parent(mux_hw, index);198 rate_ops->set_rate(rate_hw, rate, parent_rate);199 }200 201 return 0;202}203 204static int clk_composite_is_enabled(struct clk_hw *hw)205{206 struct clk_composite *composite = to_clk_composite(hw);207 const struct clk_ops *gate_ops = composite->gate_ops;208 struct clk_hw *gate_hw = composite->gate_hw;209 210 __clk_hw_set_clk(gate_hw, hw);211 212 return gate_ops->is_enabled(gate_hw);213}214 215static int clk_composite_enable(struct clk_hw *hw)216{217 struct clk_composite *composite = to_clk_composite(hw);218 const struct clk_ops *gate_ops = composite->gate_ops;219 struct clk_hw *gate_hw = composite->gate_hw;220 221 __clk_hw_set_clk(gate_hw, hw);222 223 return gate_ops->enable(gate_hw);224}225 226static void clk_composite_disable(struct clk_hw *hw)227{228 struct clk_composite *composite = to_clk_composite(hw);229 const struct clk_ops *gate_ops = composite->gate_ops;230 struct clk_hw *gate_hw = composite->gate_hw;231 232 __clk_hw_set_clk(gate_hw, hw);233 234 gate_ops->disable(gate_hw);235}236 237static struct clk_hw *__clk_hw_register_composite(struct device *dev,238 const char *name, const char * const *parent_names,239 const struct clk_parent_data *pdata, int num_parents,240 struct clk_hw *mux_hw, const struct clk_ops *mux_ops,241 struct clk_hw *rate_hw, const struct clk_ops *rate_ops,242 struct clk_hw *gate_hw, const struct clk_ops *gate_ops,243 unsigned long flags)244{245 struct clk_hw *hw;246 struct clk_init_data init = {};247 struct clk_composite *composite;248 struct clk_ops *clk_composite_ops;249 int ret;250 251 composite = kzalloc(sizeof(*composite), GFP_KERNEL);252 if (!composite)253 return ERR_PTR(-ENOMEM);254 255 init.name = name;256 init.flags = flags;257 if (parent_names)258 init.parent_names = parent_names;259 else260 init.parent_data = pdata;261 init.num_parents = num_parents;262 hw = &composite->hw;263 264 clk_composite_ops = &composite->ops;265 266 if (mux_hw && mux_ops) {267 if (!mux_ops->get_parent) {268 hw = ERR_PTR(-EINVAL);269 goto err;270 }271 272 composite->mux_hw = mux_hw;273 composite->mux_ops = mux_ops;274 clk_composite_ops->get_parent = clk_composite_get_parent;275 if (mux_ops->set_parent)276 clk_composite_ops->set_parent = clk_composite_set_parent;277 if (mux_ops->determine_rate)278 clk_composite_ops->determine_rate = clk_composite_determine_rate;279 }280 281 if (rate_hw && rate_ops) {282 if (!rate_ops->recalc_rate) {283 hw = ERR_PTR(-EINVAL);284 goto err;285 }286 clk_composite_ops->recalc_rate = clk_composite_recalc_rate;287 288 if (rate_ops->determine_rate)289 clk_composite_ops->determine_rate =290 clk_composite_determine_rate;291 else if (rate_ops->round_rate)292 clk_composite_ops->round_rate =293 clk_composite_round_rate;294 295 /* .set_rate requires either .round_rate or .determine_rate */296 if (rate_ops->set_rate) {297 if (rate_ops->determine_rate || rate_ops->round_rate)298 clk_composite_ops->set_rate =299 clk_composite_set_rate;300 else301 WARN(1, "%s: missing round_rate op is required\n",302 __func__);303 }304 305 composite->rate_hw = rate_hw;306 composite->rate_ops = rate_ops;307 }308 309 if (mux_hw && mux_ops && rate_hw && rate_ops) {310 if (mux_ops->set_parent && rate_ops->set_rate)311 clk_composite_ops->set_rate_and_parent =312 clk_composite_set_rate_and_parent;313 }314 315 if (gate_hw && gate_ops) {316 if (!gate_ops->is_enabled || !gate_ops->enable ||317 !gate_ops->disable) {318 hw = ERR_PTR(-EINVAL);319 goto err;320 }321 322 composite->gate_hw = gate_hw;323 composite->gate_ops = gate_ops;324 clk_composite_ops->is_enabled = clk_composite_is_enabled;325 clk_composite_ops->enable = clk_composite_enable;326 clk_composite_ops->disable = clk_composite_disable;327 }328 329 init.ops = clk_composite_ops;330 composite->hw.init = &init;331 332 ret = clk_hw_register(dev, hw);333 if (ret) {334 hw = ERR_PTR(ret);335 goto err;336 }337 338 if (composite->mux_hw)339 composite->mux_hw->clk = hw->clk;340 341 if (composite->rate_hw)342 composite->rate_hw->clk = hw->clk;343 344 if (composite->gate_hw)345 composite->gate_hw->clk = hw->clk;346 347 return hw;348 349err:350 kfree(composite);351 return hw;352}353 354struct clk_hw *clk_hw_register_composite(struct device *dev, const char *name,355 const char * const *parent_names, int num_parents,356 struct clk_hw *mux_hw, const struct clk_ops *mux_ops,357 struct clk_hw *rate_hw, const struct clk_ops *rate_ops,358 struct clk_hw *gate_hw, const struct clk_ops *gate_ops,359 unsigned long flags)360{361 return __clk_hw_register_composite(dev, name, parent_names, NULL,362 num_parents, mux_hw, mux_ops,363 rate_hw, rate_ops, gate_hw,364 gate_ops, flags);365}366EXPORT_SYMBOL_GPL(clk_hw_register_composite);367 368struct clk_hw *clk_hw_register_composite_pdata(struct device *dev,369 const char *name,370 const struct clk_parent_data *parent_data,371 int num_parents,372 struct clk_hw *mux_hw, const struct clk_ops *mux_ops,373 struct clk_hw *rate_hw, const struct clk_ops *rate_ops,374 struct clk_hw *gate_hw, const struct clk_ops *gate_ops,375 unsigned long flags)376{377 return __clk_hw_register_composite(dev, name, NULL, parent_data,378 num_parents, mux_hw, mux_ops,379 rate_hw, rate_ops, gate_hw,380 gate_ops, flags);381}382 383struct clk *clk_register_composite(struct device *dev, const char *name,384 const char * const *parent_names, int num_parents,385 struct clk_hw *mux_hw, const struct clk_ops *mux_ops,386 struct clk_hw *rate_hw, const struct clk_ops *rate_ops,387 struct clk_hw *gate_hw, const struct clk_ops *gate_ops,388 unsigned long flags)389{390 struct clk_hw *hw;391 392 hw = clk_hw_register_composite(dev, name, parent_names, num_parents,393 mux_hw, mux_ops, rate_hw, rate_ops, gate_hw, gate_ops,394 flags);395 if (IS_ERR(hw))396 return ERR_CAST(hw);397 return hw->clk;398}399EXPORT_SYMBOL_GPL(clk_register_composite);400 401struct clk *clk_register_composite_pdata(struct device *dev, const char *name,402 const struct clk_parent_data *parent_data,403 int num_parents,404 struct clk_hw *mux_hw, const struct clk_ops *mux_ops,405 struct clk_hw *rate_hw, const struct clk_ops *rate_ops,406 struct clk_hw *gate_hw, const struct clk_ops *gate_ops,407 unsigned long flags)408{409 struct clk_hw *hw;410 411 hw = clk_hw_register_composite_pdata(dev, name, parent_data,412 num_parents, mux_hw, mux_ops, rate_hw, rate_ops,413 gate_hw, gate_ops, flags);414 if (IS_ERR(hw))415 return ERR_CAST(hw);416 return hw->clk;417}418 419void clk_unregister_composite(struct clk *clk)420{421 struct clk_composite *composite;422 struct clk_hw *hw;423 424 hw = __clk_get_hw(clk);425 if (!hw)426 return;427 428 composite = to_clk_composite(hw);429 430 clk_unregister(clk);431 kfree(composite);432}433 434void clk_hw_unregister_composite(struct clk_hw *hw)435{436 struct clk_composite *composite;437 438 composite = to_clk_composite(hw);439 440 clk_hw_unregister(hw);441 kfree(composite);442}443EXPORT_SYMBOL_GPL(clk_hw_unregister_composite);444 445static void devm_clk_hw_release_composite(struct device *dev, void *res)446{447 clk_hw_unregister_composite(*(struct clk_hw **)res);448}449 450static struct clk_hw *__devm_clk_hw_register_composite(struct device *dev,451 const char *name, const char * const *parent_names,452 const struct clk_parent_data *pdata, int num_parents,453 struct clk_hw *mux_hw, const struct clk_ops *mux_ops,454 struct clk_hw *rate_hw, const struct clk_ops *rate_ops,455 struct clk_hw *gate_hw, const struct clk_ops *gate_ops,456 unsigned long flags)457{458 struct clk_hw **ptr, *hw;459 460 ptr = devres_alloc(devm_clk_hw_release_composite, sizeof(*ptr),461 GFP_KERNEL);462 if (!ptr)463 return ERR_PTR(-ENOMEM);464 465 hw = __clk_hw_register_composite(dev, name, parent_names, pdata,466 num_parents, mux_hw, mux_ops, rate_hw,467 rate_ops, gate_hw, gate_ops, flags);468 469 if (!IS_ERR(hw)) {470 *ptr = hw;471 devres_add(dev, ptr);472 } else {473 devres_free(ptr);474 }475 476 return hw;477}478 479struct clk_hw *devm_clk_hw_register_composite_pdata(struct device *dev,480 const char *name,481 const struct clk_parent_data *parent_data,482 int num_parents,483 struct clk_hw *mux_hw, const struct clk_ops *mux_ops,484 struct clk_hw *rate_hw, const struct clk_ops *rate_ops,485 struct clk_hw *gate_hw, const struct clk_ops *gate_ops,486 unsigned long flags)487{488 return __devm_clk_hw_register_composite(dev, name, NULL, parent_data,489 num_parents, mux_hw, mux_ops,490 rate_hw, rate_ops, gate_hw,491 gate_ops, flags);492}493