clk-rcg2.c 17 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725
  1. /*
  2. * Copyright (c) 2013, The Linux Foundation. All rights reserved.
  3. *
  4. * This software is licensed under the terms of the GNU General Public
  5. * License version 2, as published by the Free Software Foundation, and
  6. * may be copied, distributed, and modified under those terms.
  7. *
  8. * This program is distributed in the hope that it will be useful,
  9. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  11. * GNU General Public License for more details.
  12. */
  13. #include <linux/kernel.h>
  14. #include <linux/bitops.h>
  15. #include <linux/err.h>
  16. #include <linux/bug.h>
  17. #include <linux/export.h>
  18. #include <linux/clk-provider.h>
  19. #include <linux/delay.h>
  20. #include <linux/regmap.h>
  21. #include <linux/math64.h>
  22. #include <asm/div64.h>
  23. #include "clk-rcg.h"
  24. #include "common.h"
  25. #define CMD_REG 0x0
  26. #define CMD_UPDATE BIT(0)
  27. #define CMD_ROOT_EN BIT(1)
  28. #define CMD_DIRTY_CFG BIT(4)
  29. #define CMD_DIRTY_N BIT(5)
  30. #define CMD_DIRTY_M BIT(6)
  31. #define CMD_DIRTY_D BIT(7)
  32. #define CMD_ROOT_OFF BIT(31)
  33. #define CFG_REG 0x4
  34. #define CFG_SRC_DIV_SHIFT 0
  35. #define CFG_SRC_SEL_SHIFT 8
  36. #define CFG_SRC_SEL_MASK (0x7 << CFG_SRC_SEL_SHIFT)
  37. #define CFG_MODE_SHIFT 12
  38. #define CFG_MODE_MASK (0x3 << CFG_MODE_SHIFT)
  39. #define CFG_MODE_DUAL_EDGE (0x2 << CFG_MODE_SHIFT)
  40. #define M_REG 0x8
  41. #define N_REG 0xc
  42. #define D_REG 0x10
  43. static int clk_rcg2_is_enabled(struct clk_hw *hw)
  44. {
  45. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  46. u32 cmd;
  47. int ret;
  48. ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, &cmd);
  49. if (ret)
  50. return ret;
  51. return (cmd & CMD_ROOT_OFF) == 0;
  52. }
  53. static u8 clk_rcg2_get_parent(struct clk_hw *hw)
  54. {
  55. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  56. int num_parents = clk_hw_get_num_parents(hw);
  57. u32 cfg;
  58. int i, ret;
  59. ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
  60. if (ret)
  61. goto err;
  62. cfg &= CFG_SRC_SEL_MASK;
  63. cfg >>= CFG_SRC_SEL_SHIFT;
  64. for (i = 0; i < num_parents; i++)
  65. if (cfg == rcg->parent_map[i].cfg)
  66. return i;
  67. err:
  68. pr_debug("%s: Clock %s has invalid parent, using default.\n",
  69. __func__, clk_hw_get_name(hw));
  70. return 0;
  71. }
  72. static int update_config(struct clk_rcg2 *rcg)
  73. {
  74. int count, ret;
  75. u32 cmd;
  76. struct clk_hw *hw = &rcg->clkr.hw;
  77. const char *name = clk_hw_get_name(hw);
  78. ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG,
  79. CMD_UPDATE, CMD_UPDATE);
  80. if (ret)
  81. return ret;
  82. /* Wait for update to take effect */
  83. for (count = 500; count > 0; count--) {
  84. ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, &cmd);
  85. if (ret)
  86. return ret;
  87. if (!(cmd & CMD_UPDATE))
  88. return 0;
  89. udelay(1);
  90. }
  91. WARN(1, "%s: rcg didn't update its configuration.", name);
  92. return 0;
  93. }
  94. static int clk_rcg2_set_parent(struct clk_hw *hw, u8 index)
  95. {
  96. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  97. int ret;
  98. u32 cfg = rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;
  99. ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
  100. CFG_SRC_SEL_MASK, cfg);
  101. if (ret)
  102. return ret;
  103. return update_config(rcg);
  104. }
  105. /*
  106. * Calculate m/n:d rate
  107. *
  108. * parent_rate m
  109. * rate = ----------- x ---
  110. * hid_div n
  111. */
  112. static unsigned long
  113. calc_rate(unsigned long rate, u32 m, u32 n, u32 mode, u32 hid_div)
  114. {
  115. if (hid_div) {
  116. rate *= 2;
  117. rate /= hid_div + 1;
  118. }
  119. if (mode) {
  120. u64 tmp = rate;
  121. tmp *= m;
  122. do_div(tmp, n);
  123. rate = tmp;
  124. }
  125. return rate;
  126. }
  127. static unsigned long
  128. clk_rcg2_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
  129. {
  130. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  131. u32 cfg, hid_div, m = 0, n = 0, mode = 0, mask;
  132. regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
  133. if (rcg->mnd_width) {
  134. mask = BIT(rcg->mnd_width) - 1;
  135. regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + M_REG, &m);
  136. m &= mask;
  137. regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + N_REG, &n);
  138. n = ~n;
  139. n &= mask;
  140. n += m;
  141. mode = cfg & CFG_MODE_MASK;
  142. mode >>= CFG_MODE_SHIFT;
  143. }
  144. mask = BIT(rcg->hid_width) - 1;
  145. hid_div = cfg >> CFG_SRC_DIV_SHIFT;
  146. hid_div &= mask;
  147. return calc_rate(parent_rate, m, n, mode, hid_div);
  148. }
  149. static int _freq_tbl_determine_rate(struct clk_hw *hw,
  150. const struct freq_tbl *f, struct clk_rate_request *req)
  151. {
  152. unsigned long clk_flags, rate = req->rate;
  153. struct clk_hw *p;
  154. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  155. int index;
  156. f = qcom_find_freq(f, rate);
  157. if (!f)
  158. return -EINVAL;
  159. index = qcom_find_src_index(hw, rcg->parent_map, f->src);
  160. if (index < 0)
  161. return index;
  162. clk_flags = clk_hw_get_flags(hw);
  163. p = clk_hw_get_parent_by_index(hw, index);
  164. if (clk_flags & CLK_SET_RATE_PARENT) {
  165. if (f->pre_div) {
  166. rate /= 2;
  167. rate *= f->pre_div + 1;
  168. }
  169. if (f->n) {
  170. u64 tmp = rate;
  171. tmp = tmp * f->n;
  172. do_div(tmp, f->m);
  173. rate = tmp;
  174. }
  175. } else {
  176. rate = clk_hw_get_rate(p);
  177. }
  178. req->best_parent_hw = p;
  179. req->best_parent_rate = rate;
  180. req->rate = f->freq;
  181. return 0;
  182. }
  183. static int clk_rcg2_determine_rate(struct clk_hw *hw,
  184. struct clk_rate_request *req)
  185. {
  186. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  187. return _freq_tbl_determine_rate(hw, rcg->freq_tbl, req);
  188. }
  189. static int clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f)
  190. {
  191. u32 cfg, mask;
  192. struct clk_hw *hw = &rcg->clkr.hw;
  193. int ret, index = qcom_find_src_index(hw, rcg->parent_map, f->src);
  194. if (index < 0)
  195. return index;
  196. if (rcg->mnd_width && f->n) {
  197. mask = BIT(rcg->mnd_width) - 1;
  198. ret = regmap_update_bits(rcg->clkr.regmap,
  199. rcg->cmd_rcgr + M_REG, mask, f->m);
  200. if (ret)
  201. return ret;
  202. ret = regmap_update_bits(rcg->clkr.regmap,
  203. rcg->cmd_rcgr + N_REG, mask, ~(f->n - f->m));
  204. if (ret)
  205. return ret;
  206. ret = regmap_update_bits(rcg->clkr.regmap,
  207. rcg->cmd_rcgr + D_REG, mask, ~f->n);
  208. if (ret)
  209. return ret;
  210. }
  211. mask = BIT(rcg->hid_width) - 1;
  212. mask |= CFG_SRC_SEL_MASK | CFG_MODE_MASK;
  213. cfg = f->pre_div << CFG_SRC_DIV_SHIFT;
  214. cfg |= rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;
  215. if (rcg->mnd_width && f->n && (f->m != f->n))
  216. cfg |= CFG_MODE_DUAL_EDGE;
  217. ret = regmap_update_bits(rcg->clkr.regmap,
  218. rcg->cmd_rcgr + CFG_REG, mask, cfg);
  219. if (ret)
  220. return ret;
  221. return update_config(rcg);
  222. }
  223. static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate)
  224. {
  225. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  226. const struct freq_tbl *f;
  227. f = qcom_find_freq(rcg->freq_tbl, rate);
  228. if (!f)
  229. return -EINVAL;
  230. return clk_rcg2_configure(rcg, f);
  231. }
  232. static int clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate,
  233. unsigned long parent_rate)
  234. {
  235. return __clk_rcg2_set_rate(hw, rate);
  236. }
  237. static int clk_rcg2_set_rate_and_parent(struct clk_hw *hw,
  238. unsigned long rate, unsigned long parent_rate, u8 index)
  239. {
  240. return __clk_rcg2_set_rate(hw, rate);
  241. }
  242. const struct clk_ops clk_rcg2_ops = {
  243. .is_enabled = clk_rcg2_is_enabled,
  244. .get_parent = clk_rcg2_get_parent,
  245. .set_parent = clk_rcg2_set_parent,
  246. .recalc_rate = clk_rcg2_recalc_rate,
  247. .determine_rate = clk_rcg2_determine_rate,
  248. .set_rate = clk_rcg2_set_rate,
  249. .set_rate_and_parent = clk_rcg2_set_rate_and_parent,
  250. };
  251. EXPORT_SYMBOL_GPL(clk_rcg2_ops);
  252. static int clk_rcg2_shared_force_enable(struct clk_hw *hw, unsigned long rate)
  253. {
  254. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  255. const char *name = clk_hw_get_name(hw);
  256. int ret, count;
  257. /* force enable RCG */
  258. ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG,
  259. CMD_ROOT_EN, CMD_ROOT_EN);
  260. if (ret)
  261. return ret;
  262. /* wait for RCG to turn ON */
  263. for (count = 500; count > 0; count--) {
  264. ret = clk_rcg2_is_enabled(hw);
  265. if (ret)
  266. break;
  267. udelay(1);
  268. }
  269. if (!count)
  270. pr_err("%s: RCG did not turn on\n", name);
  271. /* set clock rate */
  272. ret = __clk_rcg2_set_rate(hw, rate);
  273. if (ret)
  274. return ret;
  275. /* clear force enable RCG */
  276. return regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG,
  277. CMD_ROOT_EN, 0);
  278. }
  279. static int clk_rcg2_shared_set_rate(struct clk_hw *hw, unsigned long rate,
  280. unsigned long parent_rate)
  281. {
  282. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  283. /* cache the rate */
  284. rcg->current_freq = rate;
  285. if (!__clk_is_enabled(hw->clk))
  286. return 0;
  287. return clk_rcg2_shared_force_enable(hw, rcg->current_freq);
  288. }
  289. static unsigned long
  290. clk_rcg2_shared_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
  291. {
  292. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  293. return rcg->current_freq = clk_rcg2_recalc_rate(hw, parent_rate);
  294. }
  295. static int clk_rcg2_shared_enable(struct clk_hw *hw)
  296. {
  297. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  298. return clk_rcg2_shared_force_enable(hw, rcg->current_freq);
  299. }
  300. static void clk_rcg2_shared_disable(struct clk_hw *hw)
  301. {
  302. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  303. /* switch to XO, which is the lowest entry in the freq table */
  304. clk_rcg2_shared_set_rate(hw, rcg->freq_tbl[0].freq, 0);
  305. }
  306. const struct clk_ops clk_rcg2_shared_ops = {
  307. .enable = clk_rcg2_shared_enable,
  308. .disable = clk_rcg2_shared_disable,
  309. .get_parent = clk_rcg2_get_parent,
  310. .recalc_rate = clk_rcg2_shared_recalc_rate,
  311. .determine_rate = clk_rcg2_determine_rate,
  312. .set_rate = clk_rcg2_shared_set_rate,
  313. };
  314. EXPORT_SYMBOL_GPL(clk_rcg2_shared_ops);
  315. struct frac_entry {
  316. int num;
  317. int den;
  318. };
  319. static const struct frac_entry frac_table_675m[] = { /* link rate of 270M */
  320. { 52, 295 }, /* 119 M */
  321. { 11, 57 }, /* 130.25 M */
  322. { 63, 307 }, /* 138.50 M */
  323. { 11, 50 }, /* 148.50 M */
  324. { 47, 206 }, /* 154 M */
  325. { 31, 100 }, /* 205.25 M */
  326. { 107, 269 }, /* 268.50 M */
  327. { },
  328. };
  329. static struct frac_entry frac_table_810m[] = { /* Link rate of 162M */
  330. { 31, 211 }, /* 119 M */
  331. { 32, 199 }, /* 130.25 M */
  332. { 63, 307 }, /* 138.50 M */
  333. { 11, 60 }, /* 148.50 M */
  334. { 50, 263 }, /* 154 M */
  335. { 31, 120 }, /* 205.25 M */
  336. { 119, 359 }, /* 268.50 M */
  337. { },
  338. };
  339. static int clk_edp_pixel_set_rate(struct clk_hw *hw, unsigned long rate,
  340. unsigned long parent_rate)
  341. {
  342. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  343. struct freq_tbl f = *rcg->freq_tbl;
  344. const struct frac_entry *frac;
  345. int delta = 100000;
  346. s64 src_rate = parent_rate;
  347. s64 request;
  348. u32 mask = BIT(rcg->hid_width) - 1;
  349. u32 hid_div;
  350. if (src_rate == 810000000)
  351. frac = frac_table_810m;
  352. else
  353. frac = frac_table_675m;
  354. for (; frac->num; frac++) {
  355. request = rate;
  356. request *= frac->den;
  357. request = div_s64(request, frac->num);
  358. if ((src_rate < (request - delta)) ||
  359. (src_rate > (request + delta)))
  360. continue;
  361. regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
  362. &hid_div);
  363. f.pre_div = hid_div;
  364. f.pre_div >>= CFG_SRC_DIV_SHIFT;
  365. f.pre_div &= mask;
  366. f.m = frac->num;
  367. f.n = frac->den;
  368. return clk_rcg2_configure(rcg, &f);
  369. }
  370. return -EINVAL;
  371. }
  372. static int clk_edp_pixel_set_rate_and_parent(struct clk_hw *hw,
  373. unsigned long rate, unsigned long parent_rate, u8 index)
  374. {
  375. /* Parent index is set statically in frequency table */
  376. return clk_edp_pixel_set_rate(hw, rate, parent_rate);
  377. }
  378. static int clk_edp_pixel_determine_rate(struct clk_hw *hw,
  379. struct clk_rate_request *req)
  380. {
  381. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  382. const struct freq_tbl *f = rcg->freq_tbl;
  383. const struct frac_entry *frac;
  384. int delta = 100000;
  385. s64 request;
  386. u32 mask = BIT(rcg->hid_width) - 1;
  387. u32 hid_div;
  388. int index = qcom_find_src_index(hw, rcg->parent_map, f->src);
  389. /* Force the correct parent */
  390. req->best_parent_hw = clk_hw_get_parent_by_index(hw, index);
  391. req->best_parent_rate = clk_hw_get_rate(req->best_parent_hw);
  392. if (req->best_parent_rate == 810000000)
  393. frac = frac_table_810m;
  394. else
  395. frac = frac_table_675m;
  396. for (; frac->num; frac++) {
  397. request = req->rate;
  398. request *= frac->den;
  399. request = div_s64(request, frac->num);
  400. if ((req->best_parent_rate < (request - delta)) ||
  401. (req->best_parent_rate > (request + delta)))
  402. continue;
  403. regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
  404. &hid_div);
  405. hid_div >>= CFG_SRC_DIV_SHIFT;
  406. hid_div &= mask;
  407. req->rate = calc_rate(req->best_parent_rate,
  408. frac->num, frac->den,
  409. !!frac->den, hid_div);
  410. return 0;
  411. }
  412. return -EINVAL;
  413. }
  414. const struct clk_ops clk_edp_pixel_ops = {
  415. .is_enabled = clk_rcg2_is_enabled,
  416. .get_parent = clk_rcg2_get_parent,
  417. .set_parent = clk_rcg2_set_parent,
  418. .recalc_rate = clk_rcg2_recalc_rate,
  419. .set_rate = clk_edp_pixel_set_rate,
  420. .set_rate_and_parent = clk_edp_pixel_set_rate_and_parent,
  421. .determine_rate = clk_edp_pixel_determine_rate,
  422. };
  423. EXPORT_SYMBOL_GPL(clk_edp_pixel_ops);
  424. static int clk_byte_determine_rate(struct clk_hw *hw,
  425. struct clk_rate_request *req)
  426. {
  427. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  428. const struct freq_tbl *f = rcg->freq_tbl;
  429. int index = qcom_find_src_index(hw, rcg->parent_map, f->src);
  430. unsigned long parent_rate, div;
  431. u32 mask = BIT(rcg->hid_width) - 1;
  432. struct clk_hw *p;
  433. if (req->rate == 0)
  434. return -EINVAL;
  435. req->best_parent_hw = p = clk_hw_get_parent_by_index(hw, index);
  436. req->best_parent_rate = parent_rate = clk_hw_round_rate(p, req->rate);
  437. div = DIV_ROUND_UP((2 * parent_rate), req->rate) - 1;
  438. div = min_t(u32, div, mask);
  439. req->rate = calc_rate(parent_rate, 0, 0, 0, div);
  440. return 0;
  441. }
  442. static int clk_byte_set_rate(struct clk_hw *hw, unsigned long rate,
  443. unsigned long parent_rate)
  444. {
  445. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  446. struct freq_tbl f = *rcg->freq_tbl;
  447. unsigned long div;
  448. u32 mask = BIT(rcg->hid_width) - 1;
  449. div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
  450. div = min_t(u32, div, mask);
  451. f.pre_div = div;
  452. return clk_rcg2_configure(rcg, &f);
  453. }
  454. static int clk_byte_set_rate_and_parent(struct clk_hw *hw,
  455. unsigned long rate, unsigned long parent_rate, u8 index)
  456. {
  457. /* Parent index is set statically in frequency table */
  458. return clk_byte_set_rate(hw, rate, parent_rate);
  459. }
  460. const struct clk_ops clk_byte_ops = {
  461. .is_enabled = clk_rcg2_is_enabled,
  462. .get_parent = clk_rcg2_get_parent,
  463. .set_parent = clk_rcg2_set_parent,
  464. .recalc_rate = clk_rcg2_recalc_rate,
  465. .set_rate = clk_byte_set_rate,
  466. .set_rate_and_parent = clk_byte_set_rate_and_parent,
  467. .determine_rate = clk_byte_determine_rate,
  468. };
  469. EXPORT_SYMBOL_GPL(clk_byte_ops);
  470. static int clk_byte2_determine_rate(struct clk_hw *hw,
  471. struct clk_rate_request *req)
  472. {
  473. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  474. unsigned long parent_rate, div;
  475. u32 mask = BIT(rcg->hid_width) - 1;
  476. struct clk_hw *p;
  477. unsigned long rate = req->rate;
  478. if (rate == 0)
  479. return -EINVAL;
  480. p = req->best_parent_hw;
  481. req->best_parent_rate = parent_rate = clk_hw_round_rate(p, rate);
  482. div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
  483. div = min_t(u32, div, mask);
  484. req->rate = calc_rate(parent_rate, 0, 0, 0, div);
  485. return 0;
  486. }
  487. static int clk_byte2_set_rate(struct clk_hw *hw, unsigned long rate,
  488. unsigned long parent_rate)
  489. {
  490. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  491. struct freq_tbl f = { 0 };
  492. unsigned long div;
  493. int i, num_parents = clk_hw_get_num_parents(hw);
  494. u32 mask = BIT(rcg->hid_width) - 1;
  495. u32 cfg;
  496. div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
  497. div = min_t(u32, div, mask);
  498. f.pre_div = div;
  499. regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
  500. cfg &= CFG_SRC_SEL_MASK;
  501. cfg >>= CFG_SRC_SEL_SHIFT;
  502. for (i = 0; i < num_parents; i++) {
  503. if (cfg == rcg->parent_map[i].cfg) {
  504. f.src = rcg->parent_map[i].src;
  505. return clk_rcg2_configure(rcg, &f);
  506. }
  507. }
  508. return -EINVAL;
  509. }
  510. static int clk_byte2_set_rate_and_parent(struct clk_hw *hw,
  511. unsigned long rate, unsigned long parent_rate, u8 index)
  512. {
  513. /* Read the hardware to determine parent during set_rate */
  514. return clk_byte2_set_rate(hw, rate, parent_rate);
  515. }
  516. const struct clk_ops clk_byte2_ops = {
  517. .is_enabled = clk_rcg2_is_enabled,
  518. .get_parent = clk_rcg2_get_parent,
  519. .set_parent = clk_rcg2_set_parent,
  520. .recalc_rate = clk_rcg2_recalc_rate,
  521. .set_rate = clk_byte2_set_rate,
  522. .set_rate_and_parent = clk_byte2_set_rate_and_parent,
  523. .determine_rate = clk_byte2_determine_rate,
  524. };
  525. EXPORT_SYMBOL_GPL(clk_byte2_ops);
  526. static const struct frac_entry frac_table_pixel[] = {
  527. { 3, 8 },
  528. { 2, 9 },
  529. { 4, 9 },
  530. { 1, 1 },
  531. { }
  532. };
  533. static int clk_pixel_determine_rate(struct clk_hw *hw,
  534. struct clk_rate_request *req)
  535. {
  536. unsigned long request, src_rate;
  537. int delta = 100000;
  538. const struct frac_entry *frac = frac_table_pixel;
  539. for (; frac->num; frac++) {
  540. request = (req->rate * frac->den) / frac->num;
  541. src_rate = clk_hw_round_rate(req->best_parent_hw, request);
  542. if ((src_rate < (request - delta)) ||
  543. (src_rate > (request + delta)))
  544. continue;
  545. req->best_parent_rate = src_rate;
  546. req->rate = (src_rate * frac->num) / frac->den;
  547. return 0;
  548. }
  549. return -EINVAL;
  550. }
  551. static int clk_pixel_set_rate(struct clk_hw *hw, unsigned long rate,
  552. unsigned long parent_rate)
  553. {
  554. struct clk_rcg2 *rcg = to_clk_rcg2(hw);
  555. struct freq_tbl f = { 0 };
  556. const struct frac_entry *frac = frac_table_pixel;
  557. unsigned long request;
  558. int delta = 100000;
  559. u32 mask = BIT(rcg->hid_width) - 1;
  560. u32 hid_div, cfg;
  561. int i, num_parents = clk_hw_get_num_parents(hw);
  562. regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
  563. cfg &= CFG_SRC_SEL_MASK;
  564. cfg >>= CFG_SRC_SEL_SHIFT;
  565. for (i = 0; i < num_parents; i++)
  566. if (cfg == rcg->parent_map[i].cfg) {
  567. f.src = rcg->parent_map[i].src;
  568. break;
  569. }
  570. for (; frac->num; frac++) {
  571. request = (rate * frac->den) / frac->num;
  572. if ((parent_rate < (request - delta)) ||
  573. (parent_rate > (request + delta)))
  574. continue;
  575. regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
  576. &hid_div);
  577. f.pre_div = hid_div;
  578. f.pre_div >>= CFG_SRC_DIV_SHIFT;
  579. f.pre_div &= mask;
  580. f.m = frac->num;
  581. f.n = frac->den;
  582. return clk_rcg2_configure(rcg, &f);
  583. }
  584. return -EINVAL;
  585. }
  586. static int clk_pixel_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
  587. unsigned long parent_rate, u8 index)
  588. {
  589. return clk_pixel_set_rate(hw, rate, parent_rate);
  590. }
  591. const struct clk_ops clk_pixel_ops = {
  592. .is_enabled = clk_rcg2_is_enabled,
  593. .get_parent = clk_rcg2_get_parent,
  594. .set_parent = clk_rcg2_set_parent,
  595. .recalc_rate = clk_rcg2_recalc_rate,
  596. .set_rate = clk_pixel_set_rate,
  597. .set_rate_and_parent = clk_pixel_set_rate_and_parent,
  598. .determine_rate = clk_pixel_determine_rate,
  599. };
  600. EXPORT_SYMBOL_GPL(clk_pixel_ops);