clk-composite.c 11 KB

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