clk_versal.c 16 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749
  1. // SPDX-License-Identifier: GPL-2.0+
  2. /*
  3. * (C) Copyright 2019 Xilinx, Inc.
  4. * Siva Durga Prasad Paladugu <siva.durga.paladugu@xilinx.com>
  5. */
  6. #include <common.h>
  7. #include <log.h>
  8. #include <asm/cache.h>
  9. #include <asm/ptrace.h>
  10. #include <dm/device_compat.h>
  11. #include <linux/bitops.h>
  12. #include <linux/bitfield.h>
  13. #include <malloc.h>
  14. #include <clk-uclass.h>
  15. #include <clk.h>
  16. #include <dm.h>
  17. #include <asm/arch/sys_proto.h>
  18. #include <zynqmp_firmware.h>
  19. #include <linux/err.h>
  20. #define MAX_PARENT 100
  21. #define MAX_NODES 6
  22. #define MAX_NAME_LEN 50
  23. #define CLK_TYPE_SHIFT 2
  24. #define PM_API_PAYLOAD_LEN 3
  25. #define NA_PARENT 0xFFFFFFFF
  26. #define DUMMY_PARENT 0xFFFFFFFE
  27. #define CLK_TYPE_FIELD_LEN 4
  28. #define CLK_TOPOLOGY_NODE_OFFSET 16
  29. #define NODES_PER_RESP 3
  30. #define CLK_TYPE_FIELD_MASK 0xF
  31. #define CLK_FLAG_FIELD_MASK GENMASK(21, 8)
  32. #define CLK_TYPE_FLAG_FIELD_MASK GENMASK(31, 24)
  33. #define CLK_TYPE_FLAG2_FIELD_MASK GENMASK(7, 4)
  34. #define CLK_TYPE_FLAG_BITS 8
  35. #define CLK_PARENTS_ID_LEN 16
  36. #define CLK_PARENTS_ID_MASK 0xFFFF
  37. #define END_OF_TOPOLOGY_NODE 1
  38. #define END_OF_PARENTS 1
  39. #define CLK_VALID_MASK 0x1
  40. #define NODE_CLASS_SHIFT 26U
  41. #define NODE_SUBCLASS_SHIFT 20U
  42. #define NODE_TYPE_SHIFT 14U
  43. #define NODE_INDEX_SHIFT 0U
  44. #define CLK_GET_NAME_RESP_LEN 16
  45. #define CLK_GET_TOPOLOGY_RESP_WORDS 3
  46. #define CLK_GET_PARENTS_RESP_WORDS 3
  47. #define CLK_GET_ATTR_RESP_WORDS 1
  48. #define NODE_SUBCLASS_CLOCK_PLL 1
  49. #define NODE_SUBCLASS_CLOCK_OUT 2
  50. #define NODE_SUBCLASS_CLOCK_REF 3
  51. #define NODE_CLASS_CLOCK 2
  52. #define NODE_CLASS_MASK 0x3F
  53. #define CLOCK_NODE_TYPE_MUX 1
  54. #define CLOCK_NODE_TYPE_DIV 4
  55. #define CLOCK_NODE_TYPE_GATE 6
  56. enum clk_type {
  57. CLK_TYPE_OUTPUT,
  58. CLK_TYPE_EXTERNAL,
  59. };
  60. struct clock_parent {
  61. char name[MAX_NAME_LEN];
  62. int id;
  63. u32 flag;
  64. };
  65. struct clock_topology {
  66. u32 type;
  67. u32 flag;
  68. u32 type_flag;
  69. };
  70. struct versal_clock {
  71. char clk_name[MAX_NAME_LEN];
  72. u32 valid;
  73. enum clk_type type;
  74. struct clock_topology node[MAX_NODES];
  75. u32 num_nodes;
  76. struct clock_parent parent[MAX_PARENT];
  77. u32 num_parents;
  78. u32 clk_id;
  79. };
  80. struct versal_clk_priv {
  81. struct versal_clock *clk;
  82. };
  83. static ulong pl_alt_ref_clk;
  84. static ulong ref_clk;
  85. struct versal_pm_query_data {
  86. u32 qid;
  87. u32 arg1;
  88. u32 arg2;
  89. u32 arg3;
  90. };
  91. static struct versal_clock *clock;
  92. static unsigned int clock_max_idx;
  93. #define PM_QUERY_DATA 35
  94. static int versal_pm_query(struct versal_pm_query_data qdata, u32 *ret_payload)
  95. {
  96. struct pt_regs regs;
  97. regs.regs[0] = PM_SIP_SVC | PM_QUERY_DATA;
  98. regs.regs[1] = ((u64)qdata.arg1 << 32) | qdata.qid;
  99. regs.regs[2] = ((u64)qdata.arg3 << 32) | qdata.arg2;
  100. smc_call(&regs);
  101. if (ret_payload) {
  102. ret_payload[0] = (u32)regs.regs[0];
  103. ret_payload[1] = upper_32_bits(regs.regs[0]);
  104. ret_payload[2] = (u32)regs.regs[1];
  105. ret_payload[3] = upper_32_bits(regs.regs[1]);
  106. ret_payload[4] = (u32)regs.regs[2];
  107. }
  108. return qdata.qid == PM_QID_CLOCK_GET_NAME ? 0 : regs.regs[0];
  109. }
  110. static inline int versal_is_valid_clock(u32 clk_id)
  111. {
  112. if (clk_id >= clock_max_idx)
  113. return -ENODEV;
  114. return clock[clk_id].valid;
  115. }
  116. static int versal_get_clock_name(u32 clk_id, char *clk_name)
  117. {
  118. int ret;
  119. ret = versal_is_valid_clock(clk_id);
  120. if (ret == 1) {
  121. strncpy(clk_name, clock[clk_id].clk_name, MAX_NAME_LEN);
  122. return 0;
  123. }
  124. return ret == 0 ? -EINVAL : ret;
  125. }
  126. static int versal_get_clock_type(u32 clk_id, u32 *type)
  127. {
  128. int ret;
  129. ret = versal_is_valid_clock(clk_id);
  130. if (ret == 1) {
  131. *type = clock[clk_id].type;
  132. return 0;
  133. }
  134. return ret == 0 ? -EINVAL : ret;
  135. }
  136. static int versal_pm_clock_get_num_clocks(u32 *nclocks)
  137. {
  138. struct versal_pm_query_data qdata = {0};
  139. u32 ret_payload[PAYLOAD_ARG_CNT];
  140. int ret;
  141. qdata.qid = PM_QID_CLOCK_GET_NUM_CLOCKS;
  142. ret = versal_pm_query(qdata, ret_payload);
  143. *nclocks = ret_payload[1];
  144. return ret;
  145. }
  146. static int versal_pm_clock_get_name(u32 clock_id, char *name)
  147. {
  148. struct versal_pm_query_data qdata = {0};
  149. u32 ret_payload[PAYLOAD_ARG_CNT];
  150. int ret;
  151. qdata.qid = PM_QID_CLOCK_GET_NAME;
  152. qdata.arg1 = clock_id;
  153. ret = versal_pm_query(qdata, ret_payload);
  154. if (ret)
  155. return ret;
  156. memcpy(name, ret_payload, CLK_GET_NAME_RESP_LEN);
  157. return 0;
  158. }
  159. static int versal_pm_clock_get_topology(u32 clock_id, u32 index, u32 *topology)
  160. {
  161. struct versal_pm_query_data qdata = {0};
  162. u32 ret_payload[PAYLOAD_ARG_CNT];
  163. int ret;
  164. qdata.qid = PM_QID_CLOCK_GET_TOPOLOGY;
  165. qdata.arg1 = clock_id;
  166. qdata.arg2 = index;
  167. ret = versal_pm_query(qdata, ret_payload);
  168. memcpy(topology, &ret_payload[1], CLK_GET_TOPOLOGY_RESP_WORDS * 4);
  169. return ret;
  170. }
  171. static int versal_pm_clock_get_parents(u32 clock_id, u32 index, u32 *parents)
  172. {
  173. struct versal_pm_query_data qdata = {0};
  174. u32 ret_payload[PAYLOAD_ARG_CNT];
  175. int ret;
  176. qdata.qid = PM_QID_CLOCK_GET_PARENTS;
  177. qdata.arg1 = clock_id;
  178. qdata.arg2 = index;
  179. ret = versal_pm_query(qdata, ret_payload);
  180. memcpy(parents, &ret_payload[1], CLK_GET_PARENTS_RESP_WORDS * 4);
  181. return ret;
  182. }
  183. static int versal_pm_clock_get_attributes(u32 clock_id, u32 *attr)
  184. {
  185. struct versal_pm_query_data qdata = {0};
  186. u32 ret_payload[PAYLOAD_ARG_CNT];
  187. int ret;
  188. qdata.qid = PM_QID_CLOCK_GET_ATTRIBUTES;
  189. qdata.arg1 = clock_id;
  190. ret = versal_pm_query(qdata, ret_payload);
  191. memcpy(attr, &ret_payload[1], CLK_GET_ATTR_RESP_WORDS * 4);
  192. return ret;
  193. }
  194. static int __versal_clock_get_topology(struct clock_topology *topology,
  195. u32 *data, u32 *nnodes)
  196. {
  197. int i;
  198. for (i = 0; i < PM_API_PAYLOAD_LEN; i++) {
  199. if (!(data[i] & CLK_TYPE_FIELD_MASK))
  200. return END_OF_TOPOLOGY_NODE;
  201. topology[*nnodes].type = data[i] & CLK_TYPE_FIELD_MASK;
  202. topology[*nnodes].flag = FIELD_GET(CLK_FLAG_FIELD_MASK,
  203. data[i]);
  204. topology[*nnodes].type_flag =
  205. FIELD_GET(CLK_TYPE_FLAG_FIELD_MASK, data[i]);
  206. topology[*nnodes].type_flag |=
  207. FIELD_GET(CLK_TYPE_FLAG2_FIELD_MASK, data[i]) <<
  208. CLK_TYPE_FLAG_BITS;
  209. debug("topology type:0x%x, flag:0x%x, type_flag:0x%x\n",
  210. topology[*nnodes].type, topology[*nnodes].flag,
  211. topology[*nnodes].type_flag);
  212. (*nnodes)++;
  213. }
  214. return 0;
  215. }
  216. static int versal_clock_get_topology(u32 clk_id,
  217. struct clock_topology *topology,
  218. u32 *num_nodes)
  219. {
  220. int j, ret;
  221. u32 pm_resp[PM_API_PAYLOAD_LEN] = {0};
  222. *num_nodes = 0;
  223. for (j = 0; j <= MAX_NODES; j += 3) {
  224. ret = versal_pm_clock_get_topology(clock[clk_id].clk_id, j,
  225. pm_resp);
  226. if (ret)
  227. return ret;
  228. ret = __versal_clock_get_topology(topology, pm_resp, num_nodes);
  229. if (ret == END_OF_TOPOLOGY_NODE)
  230. return 0;
  231. }
  232. return 0;
  233. }
  234. static int __versal_clock_get_parents(struct clock_parent *parents, u32 *data,
  235. u32 *nparent)
  236. {
  237. int i;
  238. struct clock_parent *parent;
  239. for (i = 0; i < PM_API_PAYLOAD_LEN; i++) {
  240. if (data[i] == NA_PARENT)
  241. return END_OF_PARENTS;
  242. parent = &parents[i];
  243. parent->id = data[i] & CLK_PARENTS_ID_MASK;
  244. if (data[i] == DUMMY_PARENT) {
  245. strcpy(parent->name, "dummy_name");
  246. parent->flag = 0;
  247. } else {
  248. parent->flag = data[i] >> CLK_PARENTS_ID_LEN;
  249. if (versal_get_clock_name(parent->id, parent->name))
  250. continue;
  251. }
  252. debug("parent name:%s\n", parent->name);
  253. *nparent += 1;
  254. }
  255. return 0;
  256. }
  257. static int versal_clock_get_parents(u32 clk_id, struct clock_parent *parents,
  258. u32 *num_parents)
  259. {
  260. int j = 0, ret;
  261. u32 pm_resp[PM_API_PAYLOAD_LEN] = {0};
  262. *num_parents = 0;
  263. do {
  264. /* Get parents from firmware */
  265. ret = versal_pm_clock_get_parents(clock[clk_id].clk_id, j,
  266. pm_resp);
  267. if (ret)
  268. return ret;
  269. ret = __versal_clock_get_parents(&parents[j], pm_resp,
  270. num_parents);
  271. if (ret == END_OF_PARENTS)
  272. return 0;
  273. j += PM_API_PAYLOAD_LEN;
  274. } while (*num_parents <= MAX_PARENT);
  275. return 0;
  276. }
  277. static u32 versal_clock_get_div(u32 clk_id)
  278. {
  279. u32 ret_payload[PAYLOAD_ARG_CNT];
  280. u32 div;
  281. xilinx_pm_request(PM_CLOCK_GETDIVIDER, clk_id, 0, 0, 0, ret_payload);
  282. div = ret_payload[1];
  283. return div;
  284. }
  285. static u32 versal_clock_set_div(u32 clk_id, u32 div)
  286. {
  287. u32 ret_payload[PAYLOAD_ARG_CNT];
  288. xilinx_pm_request(PM_CLOCK_SETDIVIDER, clk_id, div, 0, 0, ret_payload);
  289. return div;
  290. }
  291. static u64 versal_clock_ref(u32 clk_id)
  292. {
  293. u32 ret_payload[PAYLOAD_ARG_CNT];
  294. int ref;
  295. xilinx_pm_request(PM_CLOCK_GETPARENT, clk_id, 0, 0, 0, ret_payload);
  296. ref = ret_payload[0];
  297. if (!(ref & 1))
  298. return ref_clk;
  299. if (ref & 2)
  300. return pl_alt_ref_clk;
  301. return 0;
  302. }
  303. static u64 versal_clock_get_pll_rate(u32 clk_id)
  304. {
  305. u32 ret_payload[PAYLOAD_ARG_CNT];
  306. u32 fbdiv;
  307. u32 res;
  308. u32 frac;
  309. u64 freq;
  310. u32 parent_rate, parent_id;
  311. u32 id = clk_id & 0xFFF;
  312. xilinx_pm_request(PM_CLOCK_GETSTATE, clk_id, 0, 0, 0, ret_payload);
  313. res = ret_payload[1];
  314. if (!res) {
  315. printf("0%x PLL not enabled\n", clk_id);
  316. return 0;
  317. }
  318. parent_id = clock[clock[id].parent[0].id].clk_id;
  319. parent_rate = versal_clock_ref(parent_id);
  320. xilinx_pm_request(PM_CLOCK_GETDIVIDER, clk_id, 0, 0, 0, ret_payload);
  321. fbdiv = ret_payload[1];
  322. xilinx_pm_request(PM_CLOCK_PLL_GETPARAM, clk_id, 2, 0, 0, ret_payload);
  323. frac = ret_payload[1];
  324. freq = (fbdiv * parent_rate) >> (1 << frac);
  325. return freq;
  326. }
  327. static u32 versal_clock_mux(u32 clk_id)
  328. {
  329. int i;
  330. u32 id = clk_id & 0xFFF;
  331. for (i = 0; i < clock[id].num_nodes; i++)
  332. if (clock[id].node[i].type == CLOCK_NODE_TYPE_MUX)
  333. return 1;
  334. return 0;
  335. }
  336. static u32 versal_clock_get_parentid(u32 clk_id)
  337. {
  338. u32 parent_id = 0;
  339. u32 ret_payload[PAYLOAD_ARG_CNT];
  340. u32 id = clk_id & 0xFFF;
  341. if (versal_clock_mux(clk_id)) {
  342. xilinx_pm_request(PM_CLOCK_GETPARENT, clk_id, 0, 0, 0,
  343. ret_payload);
  344. parent_id = ret_payload[1];
  345. }
  346. debug("parent_id:0x%x\n", clock[clock[id].parent[parent_id].id].clk_id);
  347. return clock[clock[id].parent[parent_id].id].clk_id;
  348. }
  349. static u32 versal_clock_gate(u32 clk_id)
  350. {
  351. u32 id = clk_id & 0xFFF;
  352. int i;
  353. for (i = 0; i < clock[id].num_nodes; i++)
  354. if (clock[id].node[i].type == CLOCK_NODE_TYPE_GATE)
  355. return 1;
  356. return 0;
  357. }
  358. static u32 versal_clock_div(u32 clk_id)
  359. {
  360. int i;
  361. u32 id = clk_id & 0xFFF;
  362. for (i = 0; i < clock[id].num_nodes; i++)
  363. if (clock[id].node[i].type == CLOCK_NODE_TYPE_DIV)
  364. return 1;
  365. return 0;
  366. }
  367. static u32 versal_clock_pll(u32 clk_id, u64 *clk_rate)
  368. {
  369. if (((clk_id >> NODE_SUBCLASS_SHIFT) & NODE_CLASS_MASK) ==
  370. NODE_SUBCLASS_CLOCK_PLL &&
  371. ((clk_id >> NODE_CLASS_SHIFT) & NODE_CLASS_MASK) ==
  372. NODE_CLASS_CLOCK) {
  373. *clk_rate = versal_clock_get_pll_rate(clk_id);
  374. return 1;
  375. }
  376. return 0;
  377. }
  378. static u64 versal_clock_calc(u32 clk_id)
  379. {
  380. u32 parent_id;
  381. u64 clk_rate;
  382. u32 div;
  383. if (versal_clock_pll(clk_id, &clk_rate))
  384. return clk_rate;
  385. parent_id = versal_clock_get_parentid(clk_id);
  386. if (((parent_id >> NODE_SUBCLASS_SHIFT) &
  387. NODE_CLASS_MASK) == NODE_SUBCLASS_CLOCK_REF)
  388. return versal_clock_ref(clk_id);
  389. if (!parent_id)
  390. return 0;
  391. clk_rate = versal_clock_calc(parent_id);
  392. if (versal_clock_div(clk_id)) {
  393. div = versal_clock_get_div(clk_id);
  394. clk_rate = DIV_ROUND_CLOSEST(clk_rate, div);
  395. }
  396. return clk_rate;
  397. }
  398. static int versal_clock_get_rate(u32 clk_id, u64 *clk_rate)
  399. {
  400. if (((clk_id >> NODE_SUBCLASS_SHIFT) &
  401. NODE_CLASS_MASK) == NODE_SUBCLASS_CLOCK_REF)
  402. *clk_rate = versal_clock_ref(clk_id);
  403. if (versal_clock_pll(clk_id, clk_rate))
  404. return 0;
  405. if (((clk_id >> NODE_SUBCLASS_SHIFT) &
  406. NODE_CLASS_MASK) == NODE_SUBCLASS_CLOCK_OUT &&
  407. ((clk_id >> NODE_CLASS_SHIFT) &
  408. NODE_CLASS_MASK) == NODE_CLASS_CLOCK) {
  409. if (!versal_clock_gate(clk_id) && !versal_clock_mux(clk_id))
  410. return -EINVAL;
  411. *clk_rate = versal_clock_calc(clk_id);
  412. return 0;
  413. }
  414. return 0;
  415. }
  416. int soc_clk_dump(void)
  417. {
  418. u64 clk_rate = 0;
  419. u32 type, ret, i = 0;
  420. printf("\n ****** VERSAL CLOCKS *****\n");
  421. printf("pl_alt_ref_clk:%ld ref_clk:%ld\n", pl_alt_ref_clk, ref_clk);
  422. for (i = 0; i < clock_max_idx; i++) {
  423. debug("%s\n", clock[i].clk_name);
  424. ret = versal_get_clock_type(i, &type);
  425. if (ret || type != CLK_TYPE_OUTPUT)
  426. continue;
  427. ret = versal_clock_get_rate(clock[i].clk_id, &clk_rate);
  428. if (ret != -EINVAL)
  429. printf("clk: %s freq:%lld\n",
  430. clock[i].clk_name, clk_rate);
  431. }
  432. return 0;
  433. }
  434. static void versal_get_clock_info(void)
  435. {
  436. int i, ret;
  437. u32 attr, type = 0, nodetype, subclass, class;
  438. for (i = 0; i < clock_max_idx; i++) {
  439. ret = versal_pm_clock_get_attributes(i, &attr);
  440. if (ret)
  441. continue;
  442. clock[i].valid = attr & CLK_VALID_MASK;
  443. /* skip query for Invalid clock */
  444. ret = versal_is_valid_clock(i);
  445. if (ret != CLK_VALID_MASK)
  446. continue;
  447. clock[i].type = ((attr >> CLK_TYPE_SHIFT) & 0x1) ?
  448. CLK_TYPE_EXTERNAL : CLK_TYPE_OUTPUT;
  449. nodetype = (attr >> NODE_TYPE_SHIFT) & NODE_CLASS_MASK;
  450. subclass = (attr >> NODE_SUBCLASS_SHIFT) & NODE_CLASS_MASK;
  451. class = (attr >> NODE_CLASS_SHIFT) & NODE_CLASS_MASK;
  452. clock[i].clk_id = (class << NODE_CLASS_SHIFT) |
  453. (subclass << NODE_SUBCLASS_SHIFT) |
  454. (nodetype << NODE_TYPE_SHIFT) |
  455. (i << NODE_INDEX_SHIFT);
  456. ret = versal_pm_clock_get_name(clock[i].clk_id,
  457. clock[i].clk_name);
  458. if (ret)
  459. continue;
  460. debug("clk name:%s, Valid:%d, type:%d, clk_id:0x%x\n",
  461. clock[i].clk_name, clock[i].valid,
  462. clock[i].type, clock[i].clk_id);
  463. }
  464. /* Get topology of all clock */
  465. for (i = 0; i < clock_max_idx; i++) {
  466. ret = versal_get_clock_type(i, &type);
  467. if (ret || type != CLK_TYPE_OUTPUT)
  468. continue;
  469. debug("clk name:%s\n", clock[i].clk_name);
  470. ret = versal_clock_get_topology(i, clock[i].node,
  471. &clock[i].num_nodes);
  472. if (ret)
  473. continue;
  474. ret = versal_clock_get_parents(i, clock[i].parent,
  475. &clock[i].num_parents);
  476. if (ret)
  477. continue;
  478. }
  479. }
  480. int versal_clock_setup(void)
  481. {
  482. int ret;
  483. ret = versal_pm_clock_get_num_clocks(&clock_max_idx);
  484. if (ret)
  485. return ret;
  486. debug("%s, clock_max_idx:0x%x\n", __func__, clock_max_idx);
  487. clock = calloc(clock_max_idx, sizeof(*clock));
  488. if (!clock)
  489. return -ENOMEM;
  490. versal_get_clock_info();
  491. return 0;
  492. }
  493. static int versal_clock_get_freq_by_name(char *name, struct udevice *dev,
  494. ulong *freq)
  495. {
  496. struct clk clk;
  497. int ret;
  498. ret = clk_get_by_name(dev, name, &clk);
  499. if (ret < 0) {
  500. dev_err(dev, "failed to get %s\n", name);
  501. return ret;
  502. }
  503. *freq = clk_get_rate(&clk);
  504. if (IS_ERR_VALUE(*freq)) {
  505. dev_err(dev, "failed to get rate %s\n", name);
  506. return -EINVAL;
  507. }
  508. return 0;
  509. }
  510. static int versal_clk_probe(struct udevice *dev)
  511. {
  512. int ret;
  513. struct versal_clk_priv *priv = dev_get_priv(dev);
  514. debug("%s\n", __func__);
  515. ret = versal_clock_get_freq_by_name("pl_alt_ref_clk",
  516. dev, &pl_alt_ref_clk);
  517. if (ret < 0)
  518. return -EINVAL;
  519. ret = versal_clock_get_freq_by_name("ref_clk", dev, &ref_clk);
  520. if (ret < 0)
  521. return -EINVAL;
  522. versal_clock_setup();
  523. priv->clk = clock;
  524. return ret;
  525. }
  526. static ulong versal_clk_get_rate(struct clk *clk)
  527. {
  528. struct versal_clk_priv *priv = dev_get_priv(clk->dev);
  529. u32 id = clk->id;
  530. u32 clk_id;
  531. u64 clk_rate = 0;
  532. debug("%s\n", __func__);
  533. clk_id = priv->clk[id].clk_id;
  534. versal_clock_get_rate(clk_id, &clk_rate);
  535. return clk_rate;
  536. }
  537. static ulong versal_clk_set_rate(struct clk *clk, ulong rate)
  538. {
  539. struct versal_clk_priv *priv = dev_get_priv(clk->dev);
  540. u32 id = clk->id;
  541. u32 clk_id;
  542. u64 clk_rate = 0;
  543. u32 div;
  544. int ret;
  545. debug("%s\n", __func__);
  546. clk_id = priv->clk[id].clk_id;
  547. ret = versal_clock_get_rate(clk_id, &clk_rate);
  548. if (ret) {
  549. printf("Clock is not a Gate:0x%x\n", clk_id);
  550. return 0;
  551. }
  552. do {
  553. if (versal_clock_div(clk_id)) {
  554. div = versal_clock_get_div(clk_id);
  555. clk_rate *= div;
  556. div = DIV_ROUND_CLOSEST(clk_rate, rate);
  557. versal_clock_set_div(clk_id, div);
  558. debug("%s, div:%d, newrate:%lld\n", __func__,
  559. div, DIV_ROUND_CLOSEST(clk_rate, div));
  560. return DIV_ROUND_CLOSEST(clk_rate, div);
  561. }
  562. clk_id = versal_clock_get_parentid(clk_id);
  563. } while (((clk_id >> NODE_SUBCLASS_SHIFT) &
  564. NODE_CLASS_MASK) != NODE_SUBCLASS_CLOCK_REF);
  565. printf("Clock didn't has Divisors:0x%x\n", priv->clk[id].clk_id);
  566. return clk_rate;
  567. }
  568. static int versal_clk_enable(struct clk *clk)
  569. {
  570. struct versal_clk_priv *priv = dev_get_priv(clk->dev);
  571. u32 clk_id;
  572. clk_id = priv->clk[clk->id].clk_id;
  573. return xilinx_pm_request(PM_CLOCK_ENABLE, clk_id, 0, 0, 0, NULL);
  574. }
  575. static struct clk_ops versal_clk_ops = {
  576. .set_rate = versal_clk_set_rate,
  577. .get_rate = versal_clk_get_rate,
  578. .enable = versal_clk_enable,
  579. };
  580. static const struct udevice_id versal_clk_ids[] = {
  581. { .compatible = "xlnx,versal-clk" },
  582. { }
  583. };
  584. U_BOOT_DRIVER(versal_clk) = {
  585. .name = "versal-clk",
  586. .id = UCLASS_CLK,
  587. .of_match = versal_clk_ids,
  588. .probe = versal_clk_probe,
  589. .ops = &versal_clk_ops,
  590. .priv_auto = sizeof(struct versal_clk_priv),
  591. };