clk_versal.c 16 KB

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