--- /dev/null
+/*
+ * ZynqMP clock driver
+ *
+ * Copyright (C) 2016 Xilinx, Inc.
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ */
+
+#include <common.h>
+#include <linux/bitops.h>
+#include <clk-uclass.h>
+#include <dm/device.h>
+#include <clk.h>
+
+#define ZYNQMP_GEM0_REF_CTRL 0xFF5E0050
+#define ZYNQMP_IOPLL_CTRL 0xFF5E0020
+#define ZYNQMP_RPLL_CTRL 0xFF5E0030
+#define ZYNQMP_DPLL_CTRL 0xFD1A002C
+#define ZYNQMP_SIP_SVC_MMIO_WRITE 0xC2000013
+#define ZYNQMP_SIP_SVC_MMIO_WRITE 0xC2000013
+#define ZYNQMP_SIP_SVC_MMIO_WRITE 0xC2000013
+#define ZYNQMP_SIP_SVC_MMIO_READ 0xC2000014
+#define ZYNQMP_DIV_MAX_VAL 0x3F
+#define ZYNQMP_DIV1_SHFT 8
+#define ZYNQMP_DIV1_SHFT 8
+#define ZYNQMP_DIV2_SHFT 16
+#define ZYNQMP_DIV_MASK 0x3F
+#define ZYNQMP_PLL_CTRL_FBDIV_MASK 0x7F
+#define ZYNQMP_PLL_CTRL_FBDIV_SHFT 8
+#define ZYNQMP_GEM_REF_CTRL_SRC_MASK 0x7
+#define ZYNQMP_GEM0_CLK_ID 45
+#define ZYNQMP_GEM1_CLK_ID 46
+#define ZYNQMP_GEM2_CLK_ID 47
+#define ZYNQMP_GEM3_CLK_ID 48
+
+static unsigned long pss_ref_clk;
+
+static int zynqmp_calculate_divisors(unsigned long req_rate,
+ unsigned long parent_rate,
+ u32 *div1, u32 *div2)
+{
+ u32 req_div = 1;
+ u32 i;
+
+ /*
+ * calculate two divisors to get
+ * required rate and each divisor
+ * should be less than 63
+ */
+ req_div = DIV_ROUND_UP(parent_rate, req_rate);
+
+ for (i = 1; i <= req_div; i++) {
+ if ((req_div % i) == 0) {
+ *div1 = req_div / i;
+ *div2 = i;
+ if ((*div1 < ZYNQMP_DIV_MAX_VAL) &&
+ (*div2 < ZYNQMP_DIV_MAX_VAL))
+ return 0;
+ }
+ }
+
+ return -1;
+}
+
+static int zynqmp_get_periph_id(unsigned long id)
+{
+ int periph_id;
+
+ switch (id) {
+ case ZYNQMP_GEM0_CLK_ID:
+ periph_id = 0;
+ break;
+ case ZYNQMP_GEM1_CLK_ID:
+ periph_id = 1;
+ break;
+ case ZYNQMP_GEM2_CLK_ID:
+ periph_id = 2;
+ break;
+ case ZYNQMP_GEM3_CLK_ID:
+ periph_id = 3;
+ break;
+ default:
+ printf("%s, Invalid clock id:%ld\n", __func__, id);
+ return -EINVAL;
+ }
+
+ return periph_id;
+}
+
+static int zynqmp_set_clk(unsigned long id, u32 div1, u32 div2)
+{
+ struct pt_regs regs;
+ ulong reg;
+ u32 mask, value;
+
+ id = zynqmp_get_periph_id(id);
+ if (id < 0)
+ return -EINVAL;
+
+ reg = (ulong)((u32 *)ZYNQMP_GEM0_REF_CTRL + id);
+ mask = (ZYNQMP_DIV_MASK << ZYNQMP_DIV1_SHFT) |
+ (ZYNQMP_DIV_MASK << ZYNQMP_DIV2_SHFT);
+ value = (div1 << ZYNQMP_DIV1_SHFT) | (div2 << ZYNQMP_DIV2_SHFT);
+
+ debug("%s: reg:0x%lx, mask:0x%x, value:0x%x\n", __func__, reg, mask,
+ value);
+
+ regs.regs[0] = ZYNQMP_SIP_SVC_MMIO_WRITE;
+ regs.regs[1] = ((u64)mask << 32) | reg;
+ regs.regs[2] = value;
+ regs.regs[3] = 0;
+
+ smc_call(®s);
+
+ return regs.regs[0];
+}
+
+static unsigned long zynqmp_clk_get_rate(struct clk *clk)
+{
+ struct pt_regs regs;
+ ulong reg;
+ unsigned long value;
+ int id;
+
+ id = zynqmp_get_periph_id(clk->id);
+ if (id < 0)
+ return -EINVAL;
+
+ reg = (ulong)((u32 *)ZYNQMP_GEM0_REF_CTRL + id);
+
+ regs.regs[0] = ZYNQMP_SIP_SVC_MMIO_READ;
+ regs.regs[1] = reg;
+ regs.regs[2] = 0;
+ regs.regs[3] = 0;
+
+ smc_call(®s);
+
+ value = upper_32_bits(regs.regs[0]);
+
+ value &= ZYNQMP_GEM_REF_CTRL_SRC_MASK;
+
+ switch (value) {
+ case 0:
+ regs.regs[1] = ZYNQMP_IOPLL_CTRL;
+ break;
+ case 2:
+ regs.regs[1] = ZYNQMP_RPLL_CTRL;
+ break;
+ case 3:
+ regs.regs[1] = ZYNQMP_DPLL_CTRL;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ regs.regs[0] = ZYNQMP_SIP_SVC_MMIO_READ;
+ regs.regs[2] = 0;
+ regs.regs[3] = 0;
+
+ smc_call(®s);
+
+ value = upper_32_bits(regs.regs[0]) &
+ (ZYNQMP_PLL_CTRL_FBDIV_MASK <<
+ ZYNQMP_PLL_CTRL_FBDIV_SHFT);
+ value >>= ZYNQMP_PLL_CTRL_FBDIV_SHFT;
+ value *= pss_ref_clk;
+
+ return value;
+}
+
+static ulong zynqmp_clk_set_rate(struct clk *clk, unsigned long clk_rate)
+{
+ int ret;
+ u32 div1 = 0;
+ u32 div2 = 0;
+ unsigned long input_clk;
+
+ input_clk = zynqmp_clk_get_rate(clk);
+ if (IS_ERR_VALUE(input_clk)) {
+ dev_err(dev, "failed to get input_clk\n");
+ return -EINVAL;
+ }
+
+ debug("%s: i/p CLK %ld, clk_rate:0x%ld\n", __func__, input_clk,
+ clk_rate);
+
+ ret = zynqmp_calculate_divisors(clk_rate, input_clk, &div1, &div2);
+ if (ret) {
+ dev_err(dev, "failed to proper divisors\n");
+ return -EINVAL;
+ }
+
+ debug("%s: Div1:%d, Div2:%d\n", __func__, div1, div2);
+
+ ret = zynqmp_set_clk(clk->id, div1, div2);
+ if (ret) {
+ dev_err(dev, "failed to set gem clk\n");
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static int zynqmp_clk_probe(struct udevice *dev)
+{
+ struct clk clk;
+ int ret;
+
+ debug("%s\n", __func__);
+ ret = clk_get_by_name(dev, "pss_ref_clk", &clk);
+ if (ret < 0) {
+ dev_err(dev, "failed to get pss_ref_clk\n");
+ return ret;
+ }
+
+ pss_ref_clk = clk_get_rate(&clk);
+ if (IS_ERR_VALUE(pss_ref_clk)) {
+ dev_err(dev, "failed to get rate pss_ref_clk\n");
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static struct clk_ops zynqmp_clk_ops = {
+ .set_rate = zynqmp_clk_set_rate,
+ .get_rate = zynqmp_clk_get_rate,
+};
+
+static const struct udevice_id zynqmp_clk_ids[] = {
+ { .compatible = "xlnx,zynqmp-clkc" },
+ { }
+};
+
+U_BOOT_DRIVER(zynqmp_clk) = {
+ .name = "zynqmp-clk",
+ .id = UCLASS_CLK,
+ .of_match = zynqmp_clk_ids,
+ .probe = zynqmp_clk_probe,
+ .ops = &zynqmp_clk_ops,
+};