diff --git a/drivers/clk/clk_zynqmp.c b/drivers/clk/clk_zynqmp.c
index 50eaf31..247b55e 100644
--- a/drivers/clk/clk_zynqmp.c
+++ b/drivers/clk/clk_zynqmp.c
@@ -10,217 +10,595 @@
 #include <linux/bitops.h>
 #include <clk-uclass.h>
 #include <clk.h>
+#include <asm/arch/sys_proto.h>
 #include <dm.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
+DECLARE_GLOBAL_DATA_PTR;
 
-static unsigned long pss_ref_clk;
+static const resource_size_t zynqmp_crf_apb_clkc_base = 0xfd1a0020;
+static const resource_size_t zynqmp_crl_apb_clkc_base = 0xff5e0020;
+static const resource_size_t zynqmp_iou_clkc_base = 0xff180000;
 
-static int zynqmp_calculate_divisors(unsigned long req_rate,
-				     unsigned long parent_rate,
-				     u32 *div1, u32 *div2)
+/* Full power domain clocks */
+#define CRF_APB_APLL_CTRL		(zynqmp_crf_apb_clkc_base + 0x00)
+#define CRF_APB_DPLL_CTRL		(zynqmp_crf_apb_clkc_base + 0x0c)
+#define CRF_APB_VPLL_CTRL		(zynqmp_crf_apb_clkc_base + 0x18)
+#define CRF_APB_PLL_STATUS		(zynqmp_crf_apb_clkc_base + 0x24)
+#define CRF_APB_APLL_TO_LPD_CTRL	(zynqmp_crf_apb_clkc_base + 0x28)
+#define CRF_APB_DPLL_TO_LPD_CTRL	(zynqmp_crf_apb_clkc_base + 0x2c)
+#define CRF_APB_VPLL_TO_LPD_CTRL	(zynqmp_crf_apb_clkc_base + 0x30)
+/* Peripheral clocks */
+#define CRF_APB_ACPU_CTRL		(zynqmp_crf_apb_clkc_base + 0x40)
+#define CRF_APB_DBG_TRACE_CTRL		(zynqmp_crf_apb_clkc_base + 0x44)
+#define CRF_APB_DBG_FPD_CTRL		(zynqmp_crf_apb_clkc_base + 0x48)
+#define CRF_APB_DP_VIDEO_REF_CTRL	(zynqmp_crf_apb_clkc_base + 0x50)
+#define CRF_APB_DP_AUDIO_REF_CTRL	(zynqmp_crf_apb_clkc_base + 0x54)
+#define CRF_APB_DP_STC_REF_CTRL		(zynqmp_crf_apb_clkc_base + 0x5c)
+#define CRF_APB_DDR_CTRL		(zynqmp_crf_apb_clkc_base + 0x60)
+#define CRF_APB_GPU_REF_CTRL		(zynqmp_crf_apb_clkc_base + 0x64)
+#define CRF_APB_SATA_REF_CTRL		(zynqmp_crf_apb_clkc_base + 0x80)
+#define CRF_APB_PCIE_REF_CTRL		(zynqmp_crf_apb_clkc_base + 0x94)
+#define CRF_APB_GDMA_REF_CTRL		(zynqmp_crf_apb_clkc_base + 0x98)
+#define CRF_APB_DPDMA_REF_CTRL		(zynqmp_crf_apb_clkc_base + 0x9c)
+#define CRF_APB_TOPSW_MAIN_CTRL		(zynqmp_crf_apb_clkc_base + 0xa0)
+#define CRF_APB_TOPSW_LSBUS_CTRL	(zynqmp_crf_apb_clkc_base + 0xa4)
+#define CRF_APB_GTGREF0_REF_CTRL	(zynqmp_crf_apb_clkc_base + 0xa8)
+#define CRF_APB_DBG_TSTMP_CTRL		(zynqmp_crf_apb_clkc_base + 0xd8)
+
+/* Low power domain clocks */
+#define CRL_APB_IOPLL_CTRL		(zynqmp_crl_apb_clkc_base + 0x00)
+#define CRL_APB_RPLL_CTRL		(zynqmp_crl_apb_clkc_base + 0x10)
+#define CRL_APB_PLL_STATUS		(zynqmp_crl_apb_clkc_base + 0x20)
+#define CRL_APB_IOPLL_TO_FPD_CTRL	(zynqmp_crl_apb_clkc_base + 0x24)
+#define CRL_APB_RPLL_TO_FPD_CTRL	(zynqmp_crl_apb_clkc_base + 0x28)
+/* Peripheral clocks */
+#define CRL_APB_USB3_DUAL_REF_CTRL	(zynqmp_crl_apb_clkc_base + 0x2c)
+#define CRL_APB_GEM0_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x30)
+#define CRL_APB_GEM1_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x34)
+#define CRL_APB_GEM2_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x38)
+#define CRL_APB_GEM3_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x3c)
+#define CRL_APB_USB0_BUS_REF_CTRL	(zynqmp_crl_apb_clkc_base + 0x40)
+#define CRL_APB_USB1_BUS_REF_CTRL	(zynqmp_crl_apb_clkc_base + 0x44)
+#define CRL_APB_QSPI_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x48)
+#define CRL_APB_SDIO0_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x4c)
+#define CRL_APB_SDIO1_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x50)
+#define CRL_APB_UART0_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x54)
+#define CRL_APB_UART1_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x58)
+#define CRL_APB_SPI0_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x5c)
+#define CRL_APB_SPI1_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x60)
+#define CRL_APB_CAN0_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x64)
+#define CRL_APB_CAN1_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x68)
+#define CRL_APB_CPU_R5_CTRL		(zynqmp_crl_apb_clkc_base + 0x70)
+#define CRL_APB_IOU_SWITCH_CTRL		(zynqmp_crl_apb_clkc_base + 0x7c)
+#define CRL_APB_CSU_PLL_CTRL		(zynqmp_crl_apb_clkc_base + 0x80)
+#define CRL_APB_PCAP_CTRL		(zynqmp_crl_apb_clkc_base + 0x84)
+#define CRL_APB_LPD_SWITCH_CTRL		(zynqmp_crl_apb_clkc_base + 0x88)
+#define CRL_APB_LPD_LSBUS_CTRL		(zynqmp_crl_apb_clkc_base + 0x8c)
+#define CRL_APB_DBG_LPD_CTRL		(zynqmp_crl_apb_clkc_base + 0x90)
+#define CRL_APB_NAND_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x94)
+#define CRL_APB_ADMA_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x98)
+#define CRL_APB_PL0_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0xa0)
+#define CRL_APB_PL1_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0xa4)
+#define CRL_APB_PL2_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0xa8)
+#define CRL_APB_PL3_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0xac)
+#define CRL_APB_PL0_THR_CNT		(zynqmp_crl_apb_clkc_base + 0xb4)
+#define CRL_APB_PL1_THR_CNT		(zynqmp_crl_apb_clkc_base + 0xbc)
+#define CRL_APB_PL2_THR_CNT		(zynqmp_crl_apb_clkc_base + 0xc4)
+#define CRL_APB_PL3_THR_CNT		(zynqmp_crl_apb_clkc_base + 0xdc)
+#define CRL_APB_GEM_TSU_REF_CTRL	(zynqmp_crl_apb_clkc_base + 0xe0)
+#define CRL_APB_DLL_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0xe4)
+#define CRL_APB_AMS_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0xe8)
+#define CRL_APB_I2C0_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x100)
+#define CRL_APB_I2C1_REF_CTRL		(zynqmp_crl_apb_clkc_base + 0x104)
+#define CRL_APB_TIMESTAMP_REF_CTRL	(zynqmp_crl_apb_clkc_base + 0x108)
+#define IOU_SLCR_GEM_CLK_CTRL		(zynqmp_iou_clkc_base + 0x308)
+#define IOU_SLCR_CAN_MIO_CTRL		(zynqmp_iou_clkc_base + 0x304)
+#define IOU_SLCR_WDT_CLK_SEL		(zynqmp_iou_clkc_base + 0x300)
+
+#define ZYNQ_CLK_MAXDIV		0x3f
+#define CLK_CTRL_DIV1_SHIFT	16
+#define CLK_CTRL_DIV1_MASK	(ZYNQ_CLK_MAXDIV << CLK_CTRL_DIV1_SHIFT)
+#define CLK_CTRL_DIV0_SHIFT	8
+#define CLK_CTRL_DIV0_MASK	(ZYNQ_CLK_MAXDIV << CLK_CTRL_DIV0_SHIFT)
+#define CLK_CTRL_SRCSEL_SHIFT	0
+#define CLK_CTRL_SRCSEL_MASK	(0x3 << CLK_CTRL_SRCSEL_SHIFT)
+#define PLLCTRL_FBDIV_MASK	0x7f00
+#define PLLCTRL_FBDIV_SHIFT	8
+#define PLLCTRL_RESET_MASK	1
+#define PLLCTRL_RESET_SHIFT	0
+#define PLLCTRL_BYPASS_MASK	0x8
+#define PLLCTRL_BYPASS_SHFT	3
+#define PLLCTRL_POST_SRC_SHFT	24
+#define PLLCTRL_POST_SRC_MASK	(0x7 << PLLCTRL_POST_SRC_SHFT)
+
+
+#define NUM_MIO_PINS	77
+
+enum zynqmp_clk {
+	iopll, rpll,
+	apll, dpll, vpll,
+	iopll_to_fpd, rpll_to_fpd, apll_to_lpd, dpll_to_lpd, vpll_to_lpd,
+	acpu, acpu_half,
+	dbg_fpd, dbg_lpd, dbg_trace, dbg_tstmp,
+	dp_video_ref, dp_audio_ref,
+	dp_stc_ref, gdma_ref, dpdma_ref,
+	ddr_ref, sata_ref, pcie_ref,
+	gpu_ref, gpu_pp0_ref, gpu_pp1_ref,
+	topsw_main, topsw_lsbus,
+	gtgref0_ref,
+	lpd_switch, lpd_lsbus,
+	usb0_bus_ref, usb1_bus_ref, usb3_dual_ref, usb0, usb1,
+	cpu_r5, cpu_r5_core,
+	csu_spb, csu_pll, pcap,
+	iou_switch,
+	gem_tsu_ref, gem_tsu,
+	gem0_ref, gem1_ref, gem2_ref, gem3_ref,
+	gem0_rx, gem1_rx, gem2_rx, gem3_rx,
+	qspi_ref,
+	sdio0_ref, sdio1_ref,
+	uart0_ref, uart1_ref,
+	spi0_ref, spi1_ref,
+	nand_ref,
+	i2c0_ref, i2c1_ref, can0_ref, can1_ref, can0, can1,
+	dll_ref,
+	adma_ref,
+	timestamp_ref,
+	ams_ref,
+	pl0, pl1, pl2, pl3,
+	wdt,
+	clk_max,
+};
+
+static const char * const clk_names[clk_max] = {
+	"iopll", "rpll", "apll", "dpll",
+	"vpll", "iopll_to_fpd", "rpll_to_fpd",
+	"apll_to_lpd", "dpll_to_lpd", "vpll_to_lpd",
+	"acpu", "acpu_half", "dbf_fpd", "dbf_lpd",
+	"dbg_trace", "dbg_tstmp", "dp_video_ref",
+	"dp_audio_ref", "dp_stc_ref", "gdma_ref",
+	"dpdma_ref", "ddr_ref", "sata_ref", "pcie_ref",
+	"gpu_ref", "gpu_pp0_ref", "gpu_pp1_ref",
+	"topsw_main", "topsw_lsbus", "gtgref0_ref",
+	"lpd_switch", "lpd_lsbus", "usb0_bus_ref",
+	"usb1_bus_ref", "usb3_dual_ref", "usb0",
+	"usb1", "cpu_r5", "cpu_r5_core", "csu_spb",
+	"csu_pll", "pcap", "iou_switch", "gem_tsu_ref",
+	"gem_tsu", "gem0_ref", "gem1_ref", "gem2_ref",
+	"gem3_ref", "gem0_tx", "gem1_tx", "gem2_tx",
+	"gem3_tx", "qspi_ref", "sdio0_ref", "sdio1_ref",
+	"uart0_ref", "uart1_ref", "spi0_ref",
+	"spi1_ref", "nand_ref", "i2c0_ref", "i2c1_ref",
+	"can0_ref", "can1_ref", "can0", "can1",
+	"dll_ref", "adma_ref", "timestamp_ref",
+	"ams_ref", "pl0", "pl1", "pl2", "pl3", "wdt"
+};
+
+struct zynqmp_clk_priv {
+	unsigned long ps_clk_freq;
+	unsigned long video_clk;
+	unsigned long pss_alt_ref_clk;
+	unsigned long gt_crx_ref_clk;
+	unsigned long aux_ref_clk;
+};
+
+static u32 zynqmp_clk_get_register(enum zynqmp_clk id)
 {
-	u32 req_div = 1;
-	u32 i;
+	switch (id) {
+	case iopll:
+		return CRL_APB_IOPLL_CTRL;
+	case rpll:
+		return CRL_APB_RPLL_CTRL;
+	case apll:
+		return CRF_APB_APLL_CTRL;
+	case dpll:
+		return CRF_APB_DPLL_CTRL;
+	case vpll:
+		return CRF_APB_VPLL_CTRL;
+	case acpu:
+		return CRF_APB_ACPU_CTRL;
+	case ddr_ref:
+		return CRF_APB_DDR_CTRL;
+	case qspi_ref:
+		return CRL_APB_QSPI_REF_CTRL;
+	case gem0_ref:
+		return CRL_APB_GEM0_REF_CTRL;
+	case gem1_ref:
+		return CRL_APB_GEM1_REF_CTRL;
+	case gem2_ref:
+		return CRL_APB_GEM2_REF_CTRL;
+	case gem3_ref:
+		return CRL_APB_GEM3_REF_CTRL;
+	case uart0_ref:
+		return CRL_APB_UART0_REF_CTRL;
+	case uart1_ref:
+		return CRL_APB_UART1_REF_CTRL;
+	case sdio0_ref:
+		return CRL_APB_SDIO0_REF_CTRL;
+	case sdio1_ref:
+		return CRL_APB_SDIO1_REF_CTRL;
+	case spi0_ref:
+		return CRL_APB_SPI0_REF_CTRL;
+	case spi1_ref:
+		return CRL_APB_SPI1_REF_CTRL;
+	case nand_ref:
+		return CRL_APB_NAND_REF_CTRL;
+	case i2c0_ref:
+		return CRL_APB_I2C0_REF_CTRL;
+	case i2c1_ref:
+		return CRL_APB_I2C1_REF_CTRL;
+	case can0_ref:
+		return CRL_APB_CAN0_REF_CTRL;
+	case can1_ref:
+		return CRL_APB_CAN1_REF_CTRL;
+	default:
+		debug("Invalid clk id%d\n", id);
+	}
+	return 0;
+}
 
-	/*
-	 * calculate two divisors to get
-	 * required rate and each divisor
-	 * should be less than 63
-	 */
-	req_div = DIV_ROUND_UP(parent_rate, req_rate);
+static enum zynqmp_clk zynqmp_clk_get_cpu_pll(u32 clk_ctrl)
+{
+	u32 srcsel = (clk_ctrl & CLK_CTRL_SRCSEL_MASK) >>
+		      CLK_CTRL_SRCSEL_SHIFT;
 
-	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;
-		}
+	switch (srcsel) {
+	case 2:
+		return dpll;
+	case 3:
+		return vpll;
+	case 0 ... 1:
+	default:
+		return apll;
 	}
+}
+
+static enum zynqmp_clk zynqmp_clk_get_ddr_pll(u32 clk_ctrl)
+{
+	u32 srcsel = (clk_ctrl & CLK_CTRL_SRCSEL_MASK) >>
+		      CLK_CTRL_SRCSEL_SHIFT;
 
-	return -1;
+	switch (srcsel) {
+	case 1:
+		return vpll;
+	case 0:
+	default:
+		return dpll;
+	}
 }
 
-static int zynqmp_get_periph_id(unsigned long id)
+static enum zynqmp_clk zynqmp_clk_get_peripheral_pll(u32 clk_ctrl)
 {
-	int periph_id;
+	u32 srcsel = (clk_ctrl & CLK_CTRL_SRCSEL_MASK) >>
+		      CLK_CTRL_SRCSEL_SHIFT;
 
-	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;
+	switch (srcsel) {
+	case 2:
+		return rpll;
+	case 3:
+		return dpll;
+	case 0 ... 1:
 	default:
-		printf("%s, Invalid clock id:%ld\n", __func__, id);
-		return -EINVAL;
+		return iopll;
 	}
+}
+
+static ulong zynqmp_clk_get_pll_src(ulong clk_ctrl,
+				    struct zynqmp_clk_priv *priv,
+				    bool is_pre_src)
+{
+	u32 src_sel;
+
+	if (is_pre_src)
+		src_sel = (clk_ctrl & PLLCTRL_POST_SRC_MASK) >>
+			   PLLCTRL_POST_SRC_SHFT;
+	else
+		src_sel = (clk_ctrl & PLLCTRL_POST_SRC_MASK) >>
+			   PLLCTRL_POST_SRC_SHFT;
 
-	return periph_id;
+	switch (src_sel) {
+	case 4:
+		return priv->video_clk;
+	case 5:
+		return priv->pss_alt_ref_clk;
+	case 6:
+		return priv->aux_ref_clk;
+	case 7:
+		return priv->gt_crx_ref_clk;
+	case 0 ... 3:
+	default:
+	return priv->ps_clk_freq;
+	}
 }
 
-static int zynqmp_set_clk(unsigned long id, u32 div1, u32 div2)
+static ulong zynqmp_clk_get_pll_rate(struct zynqmp_clk_priv *priv,
+				     enum zynqmp_clk id)
 {
-	struct pt_regs regs;
-	ulong reg;
-	u32 mask, value;
+	u32 clk_ctrl, reset, mul;
+	ulong freq;
+	int ret;
 
-	id = zynqmp_get_periph_id(id);
-	if (id < 0)
-		return -EINVAL;
+	ret = zynqmp_mmio_read(zynqmp_clk_get_register(id), &clk_ctrl);
+	if (ret)
+		panic("%s mio read fail\n", __func__);
 
-	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);
+	if (clk_ctrl & PLLCTRL_BYPASS_MASK)
+		freq = zynqmp_clk_get_pll_src(clk_ctrl, priv, 0);
+	else
+		freq = zynqmp_clk_get_pll_src(clk_ctrl, priv, 1);
 
-	debug("%s: reg:0x%lx, mask:0x%x, value:0x%x\n", __func__, reg, mask,
-	      value);
+	reset = (clk_ctrl & PLLCTRL_RESET_MASK) >> PLLCTRL_RESET_SHIFT;
+	if (reset && !(clk_ctrl & PLLCTRL_BYPASS_MASK))
+		return 0;
 
-	regs.regs[0] = ZYNQMP_SIP_SVC_MMIO_WRITE;
-	regs.regs[1] = ((u64)mask << 32) | reg;
-	regs.regs[2] = value;
-	regs.regs[3] = 0;
+	mul = (clk_ctrl & PLLCTRL_FBDIV_MASK) >> PLLCTRL_FBDIV_SHIFT;
 
-	smc_call(&regs);
+	freq *= mul;
 
-	return regs.regs[0];
+	if (clk_ctrl & (1 << 16))
+		freq /= 2;
+
+	return freq;
 }
 
-static unsigned long zynqmp_clk_get_rate(struct clk *clk)
+static ulong zynqmp_clk_get_cpu_rate(struct zynqmp_clk_priv *priv,
+				     enum zynqmp_clk id)
 {
-	struct pt_regs regs;
-	ulong reg;
-	unsigned long value;
-	int id;
+	u32 clk_ctrl, div;
+	enum zynqmp_clk pll;
+	int ret;
 
-	id = zynqmp_get_periph_id(clk->id);
-	if (id < 0)
-		return -EINVAL;
+	ret = zynqmp_mmio_read(CRF_APB_ACPU_CTRL, &clk_ctrl);
+	if (ret)
+		panic("%s mio read fail\n", __func__);
 
-	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;
+	div = (clk_ctrl & CLK_CTRL_DIV0_MASK) >> CLK_CTRL_DIV0_SHIFT;
 
-	smc_call(&regs);
+	pll = zynqmp_clk_get_cpu_pll(clk_ctrl);
 
-	value = upper_32_bits(regs.regs[0]);
+	return DIV_ROUND_CLOSEST(zynqmp_clk_get_pll_rate(priv, pll), div);
+}
 
-	value &= ZYNQMP_GEM_REF_CTRL_SRC_MASK;
+static ulong zynqmp_clk_get_ddr_rate(struct zynqmp_clk_priv *priv)
+{
+	u32 clk_ctrl, div;
+	enum zynqmp_clk pll;
+	int ret;
 
-	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;
+	ret = zynqmp_mmio_read(CRF_APB_DDR_CTRL, &clk_ctrl);
+	if (ret)
+		panic("%s mio read fail\n", __func__);
+
+	div = (clk_ctrl & CLK_CTRL_DIV0_MASK) >> CLK_CTRL_DIV0_SHIFT;
+
+	pll = zynqmp_clk_get_ddr_pll(clk_ctrl);
+
+	return DIV_ROUND_CLOSEST(zynqmp_clk_get_pll_rate(priv, pll), div);
+}
+
+static ulong zynqmp_clk_get_peripheral_rate(struct zynqmp_clk_priv *priv,
+					  enum zynqmp_clk id, bool two_divs)
+{
+	enum zynqmp_clk pll;
+	u32 clk_ctrl, div0;
+	u32 div1 = 1;
+	int ret;
+
+	ret = zynqmp_mmio_read(zynqmp_clk_get_register(id), &clk_ctrl);
+	if (ret)
+		panic("%s mio read fail\n", __func__);
+
+	div0 = (clk_ctrl & CLK_CTRL_DIV0_MASK) >> CLK_CTRL_DIV0_SHIFT;
+	if (!div0)
+		div0 = 1;
+
+	if (two_divs) {
+		div1 = (clk_ctrl & CLK_CTRL_DIV1_MASK) >> CLK_CTRL_DIV1_SHIFT;
+		if (!div1)
+			div1 = 1;
 	}
 
+	pll = zynqmp_clk_get_peripheral_pll(clk_ctrl);
+
+	return
+		DIV_ROUND_CLOSEST(
+			DIV_ROUND_CLOSEST(
+				zynqmp_clk_get_pll_rate(priv, pll), div0),
+			div1);
+}
+
-	regs.regs[0] = ZYNQMP_SIP_SVC_MMIO_READ;
-	regs.regs[2] = 0;
-	regs.regs[3] = 0;
+static unsigned long zynqmp_clk_calc_peripheral_two_divs(ulong rate,
+						       ulong pll_rate,
+						       u32 *div0, u32 *div1)
+{
+	long new_err, best_err = (long)(~0UL >> 1);
+	ulong new_rate, best_rate = 0;
+	u32 d0, d1;
 
-	smc_call(&regs);
+	for (d0 = 1; d0 <= ZYNQ_CLK_MAXDIV; d0++) {
+		for (d1 = 1; d1 <= ZYNQ_CLK_MAXDIV >> 1; d1++) {
+			new_rate = DIV_ROUND_CLOSEST(
+					DIV_ROUND_CLOSEST(pll_rate, d0), d1);
+			new_err = abs(new_rate - rate);
 
-	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;
+			if (new_err < best_err) {
+				*div0 = d0;
+				*div1 = d1;
+				best_err = new_err;
+				best_rate = new_rate;
+			}
+		}
+	}
 
-	return value;
+	return best_rate;
 }
 
-static ulong zynqmp_clk_set_rate(struct clk *clk, unsigned long clk_rate)
+static ulong zynqmp_clk_set_peripheral_rate(struct zynqmp_clk_priv *priv,
+					  enum zynqmp_clk id, ulong rate,
+					  bool two_divs)
 {
+	enum zynqmp_clk pll;
+	u32 clk_ctrl, div0 = 0, div1 = 0;
+	ulong pll_rate, new_rate;
+	u32 reg;
 	int ret;
-	u32 div1 = 0;
-	u32 div2 = 0;
-	unsigned long input_clk;
+	u32 mask;
 
-	input_clk = zynqmp_clk_get_rate(clk);
-	if (IS_ERR_VALUE(input_clk)) {
-		dev_err(dev, "failed to get input_clk\n");
-		return -EINVAL;
+	reg = zynqmp_clk_get_register(id);
+	ret = zynqmp_mmio_read(reg, &clk_ctrl);
+	if (ret)
+		panic("%s mio read fail\n", __func__);
+
+	pll = zynqmp_clk_get_peripheral_pll(clk_ctrl);
+	pll_rate = zynqmp_clk_get_pll_rate(priv, pll);
+	clk_ctrl &= ~CLK_CTRL_DIV0_MASK;
+	if (two_divs) {
+		clk_ctrl &= ~CLK_CTRL_DIV1_MASK;
+		new_rate = zynqmp_clk_calc_peripheral_two_divs(rate, pll_rate,
+				&div0, &div1);
+		clk_ctrl |= div1 << CLK_CTRL_DIV1_SHIFT;
+	} else {
+		div0 = DIV_ROUND_CLOSEST(pll_rate, rate);
+		if (div0 > ZYNQ_CLK_MAXDIV)
+			div0 = ZYNQ_CLK_MAXDIV;
+		new_rate = DIV_ROUND_CLOSEST(rate, div0);
 	}
+	clk_ctrl |= div0 << CLK_CTRL_DIV0_SHIFT;
 
-	debug("%s: i/p CLK %ld, clk_rate:0x%ld\n", __func__, input_clk,
-	      clk_rate);
+	mask = (ZYNQ_CLK_MAXDIV << CLK_CTRL_DIV0_SHIFT) |
+	       (ZYNQ_CLK_MAXDIV << CLK_CTRL_DIV1_SHIFT);
 
-	ret = zynqmp_calculate_divisors(clk_rate, input_clk, &div1, &div2);
-	if (ret) {
-		dev_err(dev, "failed to proper divisors\n");
-		return -EINVAL;
+	ret = zynqmp_mmio_write(reg, mask, clk_ctrl);
+	if (ret)
+		panic("%s mio write fail\n", __func__);
+
+	return new_rate;
+}
+
+static ulong zynqmp_clk_get_rate(struct clk *clk)
+{
+	struct zynqmp_clk_priv *priv = dev_get_priv(clk->dev);
+	enum zynqmp_clk id = clk->id;
+	bool two_divs = false;
+
+	switch (id) {
+	case iopll ... vpll:
+		return zynqmp_clk_get_pll_rate(priv, id);
+	case acpu:
+		return zynqmp_clk_get_cpu_rate(priv, id);
+	case ddr_ref:
+		return zynqmp_clk_get_ddr_rate(priv);
+	case gem0_ref ... gem3_ref:
+	case qspi_ref ... can1_ref:
+		two_divs = true;
+		return zynqmp_clk_get_peripheral_rate(priv, id, two_divs);
+	default:
+		return -ENXIO;
 	}
+}
 
-	debug("%s: Div1:%d, Div2:%d\n", __func__, div1, div2);
+static ulong zynqmp_clk_set_rate(struct clk *clk, ulong rate)
+{
+	struct zynqmp_clk_priv *priv = dev_get_priv(clk->dev);
+	enum zynqmp_clk id = clk->id;
+	bool two_divs = true;
 
-	ret = zynqmp_set_clk(clk->id, div1, div2);
-	if (ret) {
-		dev_err(dev, "failed to set gem clk\n");
-		return -EINVAL;
+	switch (id) {
+	case gem0_ref ... gem3_ref:
+	case qspi_ref ... can1_ref:
+		return zynqmp_clk_set_peripheral_rate(priv, id,
+						      rate, two_divs);
+	default:
+		return -ENXIO;
+	}
+}
+
+int soc_clk_dump(void)
+{
+	struct udevice *dev;
+	int i, ret;
+
+	ret = uclass_get_device_by_driver(UCLASS_CLK,
+		DM_GET_DRIVER(zynqmp_clk), &dev);
+	if (ret)
+		return ret;
+
+	printf("clk\t\tfrequency\n");
+	for (i = 0; i < clk_max; i++) {
+		const char *name = clk_names[i];
+		if (name) {
+			struct clk clk;
+			unsigned long rate;
+
+			clk.id = i;
+			ret = clk_request(dev, &clk);
+			if (ret < 0)
+				return ret;
+
+			rate = clk_get_rate(&clk);
+
+			clk_free(&clk);
+
+			if ((rate == (unsigned long)-ENOSYS) ||
+			    (rate == (unsigned long)-ENXIO))
+				printf("%10s%20s\n", name, "unknown");
+			else
+				printf("%10s%20lu\n", name, rate);
+		}
 	}
 
 	return 0;
 }
 
-static int zynqmp_clk_probe(struct udevice *dev)
+static int zynqmp_get_freq_by_name(char *name, struct udevice *dev, ulong *freq)
 {
 	struct clk clk;
 	int ret;
 
-	debug("%s\n", __func__);
-	ret = clk_get_by_name(dev, "pss_ref_clk", &clk);
+	ret = clk_get_by_name(dev, name, &clk);
 	if (ret < 0) {
-		dev_err(dev, "failed to get pss_ref_clk\n");
+		dev_err(dev, "failed to get %s\n", name);
 		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");
+	*freq = clk_get_rate(&clk);
+	if (IS_ERR_VALUE(*freq)) {
+		dev_err(dev, "failed to get rate %s\n", name);
 		return -EINVAL;
 	}
 
 	return 0;
 }
+static int zynqmp_clk_probe(struct udevice *dev)
+{
+	int ret;
+	struct zynqmp_clk_priv *priv = dev_get_priv(dev);
+
+	debug("%s\n", __func__);
+	ret = zynqmp_get_freq_by_name("pss_ref_clk", dev, &priv->ps_clk_freq);
+	if (ret < 0)
+		return -EINVAL;
+
+	ret = zynqmp_get_freq_by_name("video_clk", dev, &priv->video_clk);
+	if (ret < 0)
+		return -EINVAL;
+
+	ret = zynqmp_get_freq_by_name("pss_alt_ref_clk", dev,
+				      &priv->pss_alt_ref_clk);
+	if (ret < 0)
+		return -EINVAL;
+
+	ret = zynqmp_get_freq_by_name("aux_ref_clk", dev, &priv->aux_ref_clk);
+	if (ret < 0)
+		return -EINVAL;
+
+	ret = zynqmp_get_freq_by_name("gt_crx_ref_clk", dev,
+				      &priv->gt_crx_ref_clk);
+	if (ret < 0)
+		return -EINVAL;
+
+	return 0;
+}
 
 static struct clk_ops zynqmp_clk_ops = {
 	.set_rate = zynqmp_clk_set_rate,
@@ -238,4 +616,5 @@
 	.of_match = zynqmp_clk_ids,
 	.probe = zynqmp_clk_probe,
 	.ops = &zynqmp_clk_ops,
+	.priv_auto_alloc_size = sizeof(struct zynqmp_clk_priv),
 };