feat(imx8m): add the ddr frequency change support for imx8m family

Add the DDR frequency change support.

Signed-off-by: Jacky Bai <ping.bai@nxp.com>
Change-Id: If1167785796b8678c351569b83d2922c66f6e530

12 files changed, 896 insertions, 5 deletions

diff --git a/plat/imx/common/imx_sip_svc.c b/plat/imx/common/imx_sip_svc.c
index fd54820cf..21f7ec794 100644
--- a/plat/imx/common/imx_sip_svc.c
+++ b/plat/imx/common/imx_sip_svc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -34,6 +34,10 @@ static uintptr_t imx_sip_handler(unsigned int smc_fid,
 		SMC_RET1(handle, imx_soc_info_handler(smc_fid, x1, x2, x3));
 		break;
 #endif
+#if defined(PLAT_imx8mm) || defined(PLAT_imx8mn) || defined(PLAT_imx8mp)
+	case IMX_SIP_DDR_DVFS:
+		return dram_dvfs_handler(smc_fid, handle, x1, x2, x3);
+#endif
 #if (defined(PLAT_imx8qm) || defined(PLAT_imx8qx))
 	case  IMX_SIP_SRTC:
 		return imx_srtc_handler(smc_fid, handle, x1, x2, x3, x4);
diff --git a/plat/imx/common/include/imx_sip_svc.h b/plat/imx/common/include/imx_sip_svc.h
index 6c7a760c6..abe2bbdcb 100644
--- a/plat/imx/common/include/imx_sip_svc.h
+++ b/plat/imx/common/include/imx_sip_svc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -17,6 +17,8 @@
 #define IMX_SIP_BUILDINFO			0xC2000003
 #define IMX_SIP_BUILDINFO_GET_COMMITHASH	0x00
 
+#define IMX_SIP_DDR_DVFS		0xc2000004
+
 #define IMX_SIP_SRC			0xC2000005
 #define IMX_SIP_SRC_SET_SECONDARY_BOOT	0x10
 #define IMX_SIP_SRC_IS_SECONDARY_BOOT	0x11
@@ -41,6 +43,10 @@ int imx_kernel_entry_handler(uint32_t smc_fid, u_register_t x1,
 int imx_soc_info_handler(uint32_t smc_fid, u_register_t x1,
 			 u_register_t x2, u_register_t x3);
 #endif
+#if defined(PLAT_imx8mm) || defined(PLAT_imx8mn) || defined(PLAT_imx8mp)
+int dram_dvfs_handler(uint32_t smc_fid, void *handle,
+	u_register_t x1, u_register_t x2, u_register_t x3);
+#endif
 
 #if defined(PLAT_imx8mm) || defined(PLAT_imx8mq)
 int imx_src_handler(uint32_t smc_fid, u_register_t x1,
diff --git a/plat/imx/imx8m/ddr/clock.c b/plat/imx/imx8m/ddr/clock.c
new file mode 100644
index 000000000..7fb5730dc
--- /dev/null
+++ b/plat/imx/imx8m/ddr/clock.c
@@ -0,0 +1,136 @@
+/*
+ * Copyright 2018-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+
+#include <lib/mmio.h>
+#include <platform_def.h>
+
+#define IMX_CCM_IP_BASE				(IMX_CCM_BASE + 0xa000)
+#define DRAM_SEL_CFG				(IMX_CCM_BASE + 0x9800)
+#define CCM_IP_CLK_ROOT_GEN_TAGET(i)		(IMX_CCM_IP_BASE + 0x80 * (i) + 0x00)
+#define CCM_IP_CLK_ROOT_GEN_TAGET_SET(i)	(IMX_CCM_IP_BASE + 0x80 * (i) + 0x04)
+#define CCM_IP_CLK_ROOT_GEN_TAGET_CLR(i)	(IMX_CCM_IP_BASE + 0x80 * (i) + 0x08)
+#define PLL_FREQ_800M	U(0x00ece580)
+#define PLL_FREQ_400M	U(0x00ec6984)
+#define PLL_FREQ_167M	U(0x00f5a406)
+
+void ddr_pll_bypass_100mts(void)
+{
+	/* change the clock source of dram_alt_clk_root to source 2 --100MHz */
+	mmio_write_32(CCM_IP_CLK_ROOT_GEN_TAGET_CLR(0), (0x7 << 24) | (0x7 << 16));
+	mmio_write_32(CCM_IP_CLK_ROOT_GEN_TAGET_SET(0), (0x2 << 24));
+
+	/* change the clock source of dram_apb_clk_root to source 2 --40MHz/2 */
+	mmio_write_32(CCM_IP_CLK_ROOT_GEN_TAGET_CLR(1), (0x7 << 24) | (0x7 << 16));
+	mmio_write_32(CCM_IP_CLK_ROOT_GEN_TAGET_SET(1), (0x2 << 24) | (0x1 << 16));
+
+	/* configure pll bypass mode */
+	mmio_write_32(DRAM_SEL_CFG + 0x4, BIT(24));
+}
+
+void ddr_pll_bypass_400mts(void)
+{
+	/* change the clock source of dram_alt_clk_root to source 1 --400MHz */
+	mmio_write_32(CCM_IP_CLK_ROOT_GEN_TAGET_CLR(0), (0x7 << 24) | (0x7 << 16));
+	mmio_write_32(CCM_IP_CLK_ROOT_GEN_TAGET_SET(0), (0x1 << 24) | (0x1 << 16));
+
+	/* change the clock source of dram_apb_clk_root to source 3 --160MHz/2 */
+	mmio_write_32(CCM_IP_CLK_ROOT_GEN_TAGET_CLR(1), (0x7 << 24) | (0x7 << 16));
+	mmio_write_32(CCM_IP_CLK_ROOT_GEN_TAGET_SET(1), (0x3 << 24) | (0x1 << 16));
+
+	/* configure pll bypass mode */
+	mmio_write_32(DRAM_SEL_CFG + 0x4, BIT(24));
+}
+
+void ddr_pll_unbypass(void)
+{
+	mmio_write_32(DRAM_SEL_CFG + 0x8, BIT(24));
+	mmio_write_32(CCM_IP_CLK_ROOT_GEN_TAGET_CLR(1), (0x7 << 24) | (0x7 << 16));
+	/* to source 4 --800MHz/5 */
+	mmio_write_32(CCM_IP_CLK_ROOT_GEN_TAGET_SET(1), (0x4 << 24) | (0x4 << 16));
+}
+
+#if defined(PLAT_imx8mq)
+void dram_pll_init(unsigned int drate)
+{
+	/* bypass the PLL */
+	mmio_setbits_32(HW_DRAM_PLL_CFG0, 0x30);
+
+	switch (drate) {
+	case 3200:
+		mmio_write_32(HW_DRAM_PLL_CFG2, PLL_FREQ_800M);
+		break;
+	case 1600:
+		mmio_write_32(HW_DRAM_PLL_CFG2, PLL_FREQ_400M);
+		break;
+	case 667:
+		mmio_write_32(HW_DRAM_PLL_CFG2, PLL_FREQ_167M);
+		break;
+	default:
+		break;
+	}
+
+	/* unbypass the PLL */
+	mmio_clrbits_32(HW_DRAM_PLL_CFG0, 0x30);
+	while (!(mmio_read_32(HW_DRAM_PLL_CFG0) & (1 << 31))) {
+		;
+	}
+}
+#else
+void dram_pll_init(unsigned int drate)
+{
+	/* bypass the PLL */
+	mmio_setbits_32(DRAM_PLL_CTRL, (1 << 16));
+	mmio_clrbits_32(DRAM_PLL_CTRL, (1 << 9));
+
+	switch (drate) {
+	case 2400:
+		mmio_write_32(DRAM_PLL_CTRL + 0x4, (300 << 12) | (3 << 4) | 2);
+		break;
+	case 1600:
+		mmio_write_32(DRAM_PLL_CTRL + 0x4, (400 << 12) | (3 << 4) | 3);
+		break;
+	case 1066:
+		mmio_write_32(DRAM_PLL_CTRL + 0x4, (266 << 12) | (3 << 4) | 3);
+		break;
+	case 667:
+		mmio_write_32(DRAM_PLL_CTRL + 0x4, (334 << 12) | (3 << 4) | 4);
+		break;
+	default:
+		break;
+	}
+
+	mmio_setbits_32(DRAM_PLL_CTRL, BIT(9));
+	/* wait for PLL locked */
+	while (!(mmio_read_32(DRAM_PLL_CTRL) & BIT(31))) {
+		;
+	}
+
+	/* unbypass the PLL */
+	mmio_clrbits_32(DRAM_PLL_CTRL, BIT(16));
+}
+#endif
+
+/* change the dram clock frequency */
+void dram_clock_switch(unsigned int target_drate, bool bypass_mode)
+{
+	if (bypass_mode) {
+		switch (target_drate) {
+		case 400:
+			ddr_pll_bypass_400mts();
+			break;
+		case 100:
+			ddr_pll_bypass_100mts();
+			break;
+		default:
+			ddr_pll_unbypass();
+			break;
+		}
+	} else {
+		dram_pll_init(target_drate);
+	}
+}
diff --git a/plat/imx/imx8m/ddr/ddr4_dvfs.c b/plat/imx/imx8m/ddr/ddr4_dvfs.c
new file mode 100644
index 000000000..cdc7dc2dd
--- /dev/null
+++ b/plat/imx/imx8m/ddr/ddr4_dvfs.c
@@ -0,0 +1,241 @@
+/*
+ * Copyright 2018-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+
+#include <dram.h>
+
+void ddr4_mr_write(uint32_t mr, uint32_t data, uint32_t mr_type, uint32_t rank)
+{
+	uint32_t val, mr_mirror, data_mirror;
+
+	/*
+	 * 1. Poll MRSTAT.mr_wr_busy until it is 0 to make sure
+	 * that there is no outstanding MR transAction.
+	 */
+	while (mmio_read_32(DDRC_MRSTAT(0)) & 0x1) {
+		;
+	}
+
+	/*
+	 * 2. Write the MRCTRL0.mr_type, MRCTRL0.mr_addr, MRCTRL0.mr_rank
+	 * and (for MRWs) MRCTRL1.mr_data to define the MR transaction.
+	 */
+	val = mmio_read_32(DDRC_DIMMCTL(0));
+	if ((val & 0x2) && (rank == 0x2)) {
+		mr_mirror = (mr & 0x4) | ((mr & 0x1) << 1) | ((mr & 0x2) >> 1); /* BA0, BA1 swap */
+		data_mirror = (data & 0x1607) | ((data & 0x8) << 1) | ((data & 0x10) >> 1) |
+				((data & 0x20) << 1) | ((data & 0x40) >> 1) | ((data & 0x80) << 1) |
+				 ((data & 0x100) >> 1) | ((data & 0x800) << 2) | ((data & 0x2000) >> 2) ;
+	} else {
+		mr_mirror = mr;
+		data_mirror = data;
+	}
+
+	mmio_write_32(DDRC_MRCTRL0(0), mr_type | (mr_mirror << 12) | (rank << 4));
+	mmio_write_32(DDRC_MRCTRL1(0), data_mirror);
+
+	/*
+	 * 3. In a separate APB transaction, write the MRCTRL0.mr_wr to 1.
+	 * This bit is self-clearing, and triggers the MR transaction.
+	 * The uMCTL2 then asserts the MRSTAT.mr_wr_busy while it performs
+	 * the MR transaction to SDRAM, and no further accesses can be
+	 * initiated until it is deasserted.
+	 */
+	mmio_setbits_32(DDRC_MRCTRL0(0), BIT(31));
+
+	while (mmio_read_32(DDRC_MRSTAT(0))) {
+		;
+	}
+}
+
+void dram_cfg_all_mr(struct dram_info *info, uint32_t pstate)
+{
+	uint32_t num_rank = info->num_rank;
+	/*
+	 * 15. Perform MRS commands as required to re-program
+	 * timing registers in the SDRAM for the new frequency
+	 * (in particular, CL, CWL and WR may need to be changed).
+	 */
+
+	for (int i = 1; i <= num_rank; i++) {
+		for (int j = 0; j < 6; j++) {
+			ddr4_mr_write(j, info->mr_table[pstate][j], 0, i);
+		}
+		ddr4_mr_write(6, info->mr_table[pstate][7], 0, i);
+	}
+}
+
+void sw_pstate(uint32_t pstate, uint32_t drate)
+{
+	uint32_t val;
+
+	mmio_write_32(DDRC_SWCTL(0), 0x0);
+
+	/*
+	 * Update any registers which may be required to
+	 * change for the new frequency.
+	 */
+	mmio_write_32(DDRC_MSTR2(0), pstate);
+	mmio_setbits_32(DDRC_MSTR(0), (0x1 << 29));
+
+	/*
+	 * Toggle RFSHCTL3.refresh_update_level to allow the
+	 * new refresh-related register values to propagate
+	 * to the refresh logic.
+	 */
+	val = mmio_read_32(DDRC_RFSHCTL3(0));
+	if (val & 0x2) {
+		mmio_write_32(DDRC_RFSHCTL3(0), val & 0xFFFFFFFD);
+	} else {
+		mmio_write_32(DDRC_RFSHCTL3(0), val | 0x2);
+	}
+
+	/*
+	 * 19. If required, trigger the initialization in the PHY.
+	 * If using the gen2 multiPHY, PLL initialization should
+	 * be triggered at this point. See the PHY databook for
+	 * details about the frequency change procedure.
+	 */
+	mmio_write_32(DDRC_DFIMISC(0), 0x00000000 | (pstate << 8));
+	mmio_write_32(DDRC_DFIMISC(0), 0x00000020 | (pstate << 8));
+
+	/* wait DFISTAT.dfi_init_complete to 0 */
+	while (mmio_read_32(DDRC_DFISTAT(0)) & 0x1) {
+		;
+	}
+
+	/* change the clock to the target frequency */
+	dram_clock_switch(drate, false);
+
+	mmio_write_32(DDRC_DFIMISC(0), 0x00000000 | (pstate << 8));
+
+	/* wait DFISTAT.dfi_init_complete to 1 */
+	while (!(mmio_read_32(DDRC_DFISTAT(0)) & 0x1)) {
+		;
+	}
+
+	/*
+	 * When changing frequencies the controller may violate the JEDEC
+	 * requirement that no more than 16 refreshes should be issued within
+	 * 2*tREFI. These extra refreshes are not expected to cause a problem
+	 * in the SDRAM. This issue can be avoided by waiting for at least 2*tREFI
+	 * before exiting self-refresh in step 19.
+	 */
+	udelay(14);
+
+	/* 14. Exit the self-refresh state by setting PWRCTL.selfref_sw = 0. */
+	mmio_clrbits_32(DDRC_PWRCTL(0), (1 << 5));
+
+	while ((mmio_read_32(DDRC_STAT(0)) & 0x3f) == 0x23) {
+		;
+	}
+}
+
+void ddr4_swffc(struct dram_info *info, unsigned int pstate)
+{
+	uint32_t drate = info->timing_info->fsp_table[pstate];
+
+	/*
+	 * 1. set SWCTL.sw_done to disable quasi-dynamic register
+	 * programming outside reset.
+	 */
+	mmio_write_32(DDRC_SWCTL(0), 0x0);
+
+	/*
+	 * 2. Write 0 to PCTRL_n.port_en. This blocks AXI port(s)
+	 * from taking any transaction (blocks traffic on AXI ports).
+	 */
+	mmio_write_32(DDRC_PCTRL_0(0), 0x0);
+
+	/*
+	 * 3. Poll PSTAT.rd_port_busy_n=0 and PSTAT.wr_port_busy_n=0.
+	 * Wait until all AXI ports are idle (the uMCTL2 core has to
+	 * be idle).
+	 */
+	while (mmio_read_32(DDRC_PSTAT(0)) & 0x10001) {
+		;
+	}
+
+	/*
+	 * 4. Write 0 to SBRCTL.scrub_en. Disable SBR, required only if
+	 * SBR instantiated.
+	 * 5. Poll SBRSTAT.scrub_busy=0.
+	 * 6. Set DERATEEN.derate_enable = 0, if DERATEEN.derate_eanble = 1
+	 * and the read latency (RL) value needs to change after the frequency
+	 * change (LPDDR2/3/4 only).
+	 * 7. Set DBG1.dis_hif=1 so that no new commands will be accepted by the uMCTL2.
+	 */
+	mmio_setbits_32(DDRC_DBG1(0), (0x1 << 1));
+
+	/*
+	 * 8. Poll DBGCAM.dbg_wr_q_empty and DBGCAM.dbg_rd_q_empty to ensure
+	 * that write and read data buffers are empty.
+	 */
+	while ((mmio_read_32(DDRC_DBGCAM(0)) & 0x06000000) != 0x06000000) {
+		;
+	}
+
+	/*
+	 * 9. For DDR4, update MR6 with the new tDLLK value via the Mode
+	 * Register Write signals
+	 * 10. Set DFILPCFG0.dfi_lp_en_sr = 0, if DFILPCFG0.dfi_lp_en_sr = 1,
+	 * and wait until DFISTAT.dfi_lp_ack
+	 * 11. If DFI PHY Master interface is active in uMCTL2, then disable it
+	 * 12. Wait until STAT.operating_mode[1:0]!=11 indicating that the
+	 * controller is not in self-refresh mode.
+	 */
+	while ((mmio_read_32(DDRC_STAT(0)) & 0x3) == 0x3) {
+		;
+	}
+
+	/*
+	 * 13. Assert PWRCTL.selfref_sw for the DWC_ddr_umctl2 core to enter
+	 * the self-refresh mode.
+	 */
+	mmio_setbits_32(DDRC_PWRCTL(0), (1 << 5));
+
+	/*
+	 * 14. Wait until STAT.operating_mode[1:0]==11 indicating that the
+	 * controller core is in self-refresh mode.
+	 */
+	while ((mmio_read_32(DDRC_STAT(0)) & 0x3f) != 0x23) {
+		;
+	}
+
+	sw_pstate(pstate, drate);
+	dram_cfg_all_mr(info, pstate);
+
+	/* 23. Enable HIF commands by setting DBG1.dis_hif=0. */
+	mmio_clrbits_32(DDRC_DBG1(0), (0x1 << 1));
+
+	/*
+	 * 24. Reset DERATEEN.derate_enable = 1 if DERATEEN.derate_enable
+	 * has been set to 0 in step 6.
+	 * 25. If DFI PHY Master interface was active before step 11 then
+	 * enable it back by programming DFIPHYMSTR.phymstr_en = 1'b1.
+	 * 26. Write 1 to PCTRL_n.port_en. AXI port(s) are no longer blocked
+	 * from taking transactions (Re-enable traffic on AXI ports)
+	 */
+	mmio_write_32(DDRC_PCTRL_0(0), 0x1);
+
+	/*
+	 * 27. Write 1 to SBRCTL.scrub_en. Enable SBR if desired, only
+	 * required if SBR instantiated.
+	 */
+
+	/*
+	 * set SWCTL.sw_done to enable quasi-dynamic register programming
+	 * outside reset.
+	 */
+	mmio_write_32(DDRC_SWCTL(0), 0x1);
+
+	/* wait SWSTAT.sw_done_ack to 1 */
+	while (!(mmio_read_32(DDRC_SWSTAT(0)) & 0x1)) {
+		;
+	}
+}
diff --git a/plat/imx/imx8m/ddr/dram.c b/plat/imx/imx8m/ddr/dram.c
index 6841b2d06..6ccd6fd17 100644
--- a/plat/imx/imx8m/ddr/dram.c
+++ b/plat/imx/imx8m/ddr/dram.c
@@ -4,12 +4,46 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <bl31/interrupt_mgmt.h>
+#include <common/runtime_svc.h>
 #include <lib/mmio.h>
+#include <lib/spinlock.h>
+#include <plat/common/platform.h>
 
 #include <dram.h>
 
+#define IMX_SIP_DDR_DVFS_GET_FREQ_COUNT		0x10
+#define IMX_SIP_DDR_DVFS_GET_FREQ_INFO		0x11
+
 struct dram_info dram_info;
 
+/* lock used for DDR DVFS */
+spinlock_t dfs_lock;
+
+static volatile uint32_t wfe_done;
+static volatile bool wait_ddrc_hwffc_done = true;
+static unsigned int dev_fsp = 0x1;
+
+static uint32_t fsp_init_reg[3][4] = {
+	{ DDRC_INIT3(0), DDRC_INIT4(0), DDRC_INIT6(0), DDRC_INIT7(0) },
+	{ DDRC_FREQ1_INIT3(0), DDRC_FREQ1_INIT4(0), DDRC_FREQ1_INIT6(0), DDRC_FREQ1_INIT7(0) },
+	{ DDRC_FREQ2_INIT3(0), DDRC_FREQ2_INIT4(0), DDRC_FREQ2_INIT6(0), DDRC_FREQ2_INIT7(0) },
+};
+
+static void get_mr_values(uint32_t (*mr_value)[8])
+{
+	uint32_t init_val;
+	unsigned int i, fsp_index;
+
+	for (fsp_index = 0U; fsp_index < 3U; fsp_index++) {
+		for (i = 0U; i < 4U; i++) {
+			init_val = mmio_read_32(fsp_init_reg[fsp_index][i]);
+			mr_value[fsp_index][2*i] = init_val >> 16;
+			mr_value[fsp_index][2*i + 1] = init_val & 0xFFFF;
+		}
+	}
+}
+
 /* Restore the ddrc configs */
 void dram_umctl2_init(struct dram_timing_info *timing)
 {
@@ -53,9 +87,42 @@ void dram_phy_init(struct dram_timing_info *timing)
 	}
 }
 
+/* EL3 SGI-8 IPI handler for DDR Dynamic frequency scaling */
+static uint64_t waiting_dvfs(uint32_t id, uint32_t flags,
+				void *handle, void *cookie)
+{
+	uint64_t mpidr = read_mpidr_el1();
+	unsigned int cpu_id = MPIDR_AFFLVL0_VAL(mpidr);
+	uint32_t irq;
+
+	irq = plat_ic_acknowledge_interrupt();
+	if (irq < 1022U) {
+		plat_ic_end_of_interrupt(irq);
+	}
+
+	/* set the WFE done status */
+	spin_lock(&dfs_lock);
+	wfe_done |= (1 << cpu_id * 8);
+	dsb();
+	spin_unlock(&dfs_lock);
+
+	while (1) {
+		/* ddr frequency change done */
+		if (!wait_ddrc_hwffc_done)
+			break;
+
+		wfe();
+	}
+
+	return 0;
+}
+
 void dram_info_init(unsigned long dram_timing_base)
 {
 	uint32_t ddrc_mstr, current_fsp;
+	uint32_t flags = 0;
+	uint32_t rc;
+	unsigned int i;
 
 	/* Get the dram type & rank */
 	ddrc_mstr = mmio_read_32(DDRC_MSTR(0));
@@ -68,5 +135,127 @@ void dram_info_init(unsigned long dram_timing_base)
 	dram_info.boot_fsp = current_fsp;
 	dram_info.current_fsp = current_fsp;
 
+	get_mr_values(dram_info.mr_table);
+
 	dram_info.timing_info = (struct dram_timing_info *)dram_timing_base;
+
+	/* get the num of supported fsp */
+	for (i = 0U; i < 4U; ++i) {
+		if (!dram_info.timing_info->fsp_table[i]) {
+			break;
+		}
+	}
+	dram_info.num_fsp = i;
+
+	/* check if has bypass mode support */
+	if (dram_info.timing_info->fsp_table[i-1] < 666) {
+		dram_info.bypass_mode = true;
+	} else {
+		dram_info.bypass_mode = false;
+	}
+
+	/* Register the EL3 handler for DDR DVFS */
+	set_interrupt_rm_flag(flags, NON_SECURE);
+	rc = register_interrupt_type_handler(INTR_TYPE_EL3, waiting_dvfs, flags);
+	if (rc != 0) {
+		panic();
+	}
+}
+
+
+/*
+ * For each freq return the following info:
+ *
+ * r1: data rate
+ * r2: 1 + dram_core parent
+ * r3: 1 + dram_alt parent index
+ * r4: 1 + dram_apb parent index
+ *
+ * The parent indices can be used by an OS who manages source clocks to enabled
+ * them ahead of the switch.
+ *
+ * A parent value of "0" means "don't care".
+ *
+ * Current implementation of freq switch is hardcoded in
+ * plat/imx/common/imx8m/clock.c but in theory this can be enhanced to support
+ * a wide variety of rates.
+ */
+int dram_dvfs_get_freq_info(void *handle, u_register_t index)
+{
+	switch (index) {
+	case 0:
+		 SMC_RET4(handle, dram_info.timing_info->fsp_table[0],
+			1, 0, 5);
+	case 1:
+		if (!dram_info.bypass_mode) {
+			SMC_RET4(handle, dram_info.timing_info->fsp_table[1],
+				1, 0, 0);
+		}
+		SMC_RET4(handle, dram_info.timing_info->fsp_table[1],
+			2, 2, 4);
+	case 2:
+		if (!dram_info.bypass_mode) {
+			SMC_RET4(handle, dram_info.timing_info->fsp_table[2],
+				1, 0, 0);
+		}
+		SMC_RET4(handle, dram_info.timing_info->fsp_table[2],
+			2, 3, 3);
+	case 3:
+		 SMC_RET4(handle, dram_info.timing_info->fsp_table[3],
+			1, 0, 0);
+	default:
+		SMC_RET1(handle, -3);
+	}
+}
+
+int dram_dvfs_handler(uint32_t smc_fid, void *handle,
+	u_register_t x1, u_register_t x2, u_register_t x3)
+{
+	uint64_t mpidr = read_mpidr_el1();
+	unsigned int cpu_id = MPIDR_AFFLVL0_VAL(mpidr);
+	unsigned int fsp_index = x1;
+	uint32_t online_cores = x2;
+
+	if (x1 == IMX_SIP_DDR_DVFS_GET_FREQ_COUNT) {
+		SMC_RET1(handle, dram_info.num_fsp);
+	} else if (x1 == IMX_SIP_DDR_DVFS_GET_FREQ_INFO) {
+		return dram_dvfs_get_freq_info(handle, x2);
+	} else if (x1 < 4) {
+		wait_ddrc_hwffc_done = true;
+		dsb();
+
+		/* trigger the SGI IPI to info other cores */
+		for (int i = 0; i < PLATFORM_CORE_COUNT; i++) {
+			if (cpu_id != i && (online_cores & (0x1 << (i * 8)))) {
+				plat_ic_raise_el3_sgi(0x8, i);
+			}
+		}
+
+		/* make sure all the core in WFE */
+		online_cores &= ~(0x1 << (cpu_id * 8));
+		while (1) {
+			if (online_cores == wfe_done) {
+				break;
+			}
+		}
+
+		/* flush the L1/L2 cache */
+		dcsw_op_all(DCCSW);
+
+		if (dram_info.dram_type == DDRC_LPDDR4) {
+			lpddr4_swffc(&dram_info, dev_fsp, fsp_index);
+			dev_fsp = (~dev_fsp) & 0x1;
+		} else if (dram_info.dram_type == DDRC_DDR4) {
+			ddr4_swffc(&dram_info, fsp_index);
+		}
+
+		dram_info.current_fsp = fsp_index;
+		wait_ddrc_hwffc_done = false;
+		wfe_done = 0;
+		dsb();
+		sev();
+		isb();
+	}
+
+	SMC_RET1(handle, 0);
 }
diff --git a/plat/imx/imx8m/ddr/lpddr4_dvfs.c b/plat/imx/imx8m/ddr/lpddr4_dvfs.c
new file mode 100644
index 000000000..2b4f300c7
--- /dev/null
+++ b/plat/imx/imx8m/ddr/lpddr4_dvfs.c
@@ -0,0 +1,292 @@
+/*
+ * Copyright 2018-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/mmio.h>
+
+#include <dram.h>
+
+static void lpddr4_mr_write(uint32_t mr_rank, uint32_t mr_addr, uint32_t mr_data)
+{
+	/*
+	 * 1. Poll MRSTAT.mr_wr_busy until it is 0. This checks that there
+	 * is no outstanding MR transaction. No
+	 * writes should be performed to MRCTRL0 and MRCTRL1 if MRSTAT.mr_wr_busy = 1.
+	 */
+	while (mmio_read_32(DDRC_MRSTAT(0)) & 0x1)
+		;
+
+	/*
+	 * 2. Write the MRCTRL0.mr_type, MRCTRL0.mr_addr,
+	 * MRCTRL0.mr_rank and (for MRWs)
+	 * MRCTRL1.mr_data to define the MR transaction.
+	 */
+	mmio_write_32(DDRC_MRCTRL0(0), (mr_rank << 4));
+	mmio_write_32(DDRC_MRCTRL1(0), (mr_addr << 8) | mr_data);
+	mmio_setbits_32(DDRC_MRCTRL0(0), BIT(31));
+}
+
+void lpddr4_swffc(struct dram_info *info, unsigned int init_fsp,
+	 unsigned int fsp_index)
+
+{
+	uint32_t mr, emr, emr2, emr3;
+	uint32_t mr11, mr12, mr22, mr14;
+	uint32_t val;
+	uint32_t derate_backup[3];
+	uint32_t (*mr_data)[8];
+
+	/* 1. program targetd UMCTL2_REGS_FREQ1/2/3,already done, skip it. */
+
+	/* 2. MR13.FSP-WR=1, MRW to update MR registers */
+	mr_data = info->mr_table;
+	mr = mr_data[fsp_index][0];
+	emr  = mr_data[fsp_index][1];
+	emr2 = mr_data[fsp_index][2];
+	emr3 = mr_data[fsp_index][3];
+	mr11 = mr_data[fsp_index][4];
+	mr12 = mr_data[fsp_index][5];
+	mr22 = mr_data[fsp_index][6];
+	mr14 = mr_data[fsp_index][7];
+
+	val = (init_fsp == 1) ? 0x2 << 6 : 0x1 << 6;
+	emr3 = (emr3 & 0x003f) | val | 0x0d00;
+
+	/* 12. set PWRCTL.selfref_en=0 */
+	mmio_clrbits_32(DDRC_PWRCTL(0), 0xf);
+
+	/* It is more safe to config it here */
+	mmio_clrbits_32(DDRC_DFIPHYMSTR(0), 0x1);
+
+	lpddr4_mr_write(3, 13, emr3);
+	lpddr4_mr_write(3, 1, mr);
+	lpddr4_mr_write(3, 2, emr);
+	lpddr4_mr_write(3, 3, emr2);
+	lpddr4_mr_write(3, 11, mr11);
+	lpddr4_mr_write(3, 12, mr12);
+	lpddr4_mr_write(3, 14, mr14);
+	lpddr4_mr_write(3, 22, mr22);
+
+	do {
+		val = mmio_read_32(DDRC_MRSTAT(0));
+	} while (val & 0x1);
+
+	/* 3. disable AXI ports */
+	mmio_write_32(DDRC_PCTRL_0(0), 0x0);
+
+	/* 4.Poll PSTAT.rd_port_busy_n=0 and PSTAT.wr_port_busy_n=0. */
+	do {
+		val = mmio_read_32(DDRC_PSTAT(0));
+	} while (val != 0);
+
+	/* 6.disable SBRCTL.scrub_en, skip if never enable it */
+	/* 7.poll SBRSTAT.scrub_busy  Q2: should skip phy master if never enable it */
+	/* Disable phy master */
+#ifdef DFILP_SPT
+	/* 8. disable DFI LP */
+	/* DFILPCFG0.dfi_lp_en_sr */
+	val = mmio_read_32(DDRC_DFILPCFG0(0));
+	if (val & 0x100) {
+		mmio_write_32(DDRC_DFILPCFG0(0), 0x0);
+		do {
+			val = mmio_read_32(DDRC_DFISTAT(0)); // dfi_lp_ack
+			val2 = mmio_read_32(DDRC_STAT(0)); // operating_mode
+		} while (((val & 0x2) == 0x2) && ((val2 & 0x7) == 3));
+	}
+#endif
+	/* 9. wait until in normal or power down states */
+	do {
+		/* operating_mode */
+		val = mmio_read_32(DDRC_STAT(0));
+	} while (((val & 0x7) != 1) && ((val & 0x7) != 2));
+
+	/* 10. Disable automatic derating: derate_enable */
+	val = mmio_read_32(DDRC_DERATEEN(0));
+	derate_backup[0] = val;
+	mmio_clrbits_32(DDRC_DERATEEN(0), 0x1);
+
+	val = mmio_read_32(DDRC_FREQ1_DERATEEN(0));
+	derate_backup[1] = val;
+	mmio_clrbits_32(DDRC_FREQ1_DERATEEN(0), 0x1);
+
+	val = mmio_read_32(DDRC_FREQ2_DERATEEN(0));
+	derate_backup[2] = val;
+	mmio_clrbits_32(DDRC_FREQ2_DERATEEN(0), 0x1);
+
+	/* 11. disable automatic ZQ calibration */
+	mmio_setbits_32(DDRC_ZQCTL0(0), BIT(31));
+	mmio_setbits_32(DDRC_FREQ1_ZQCTL0(0), BIT(31));
+	mmio_setbits_32(DDRC_FREQ2_ZQCTL0(0), BIT(31));
+
+	/* 12. set PWRCTL.selfref_en=0 */
+	mmio_clrbits_32(DDRC_PWRCTL(0), 0x1);
+
+	/* 13.Poll STAT.operating_mode is in "Normal" (001) or "Power-down" (010) */
+	do {
+		val = mmio_read_32(DDRC_STAT(0));
+	} while (((val & 0x7) != 1) && ((val & 0x7) != 2));
+
+	/* 14-15. trigger SW SR */
+	/* bit 5: selfref_sw, bit 6: stay_in_selfref */
+	mmio_setbits_32(DDRC_PWRCTL(0), 0x60);
+
+	/* 16. Poll STAT.selfref_state in "Self Refresh 1" */
+	do {
+		val = mmio_read_32(DDRC_STAT(0));
+	} while ((val & 0x300) != 0x100);
+
+	/* 17. disable dq */
+	mmio_setbits_32(DDRC_DBG1(0), 0x1);
+
+	/* 18. Poll DBGCAM.wr_data_pipeline_empty and DBGCAM.rd_data_pipeline_empty */
+	do {
+		val = mmio_read_32(DDRC_DBGCAM(0));
+		val &= 0x30000000;
+	} while (val != 0x30000000);
+
+	/* 19. change MR13.FSP-OP to new FSP and MR13.VRCG to high current */
+	emr3 = (((~init_fsp) & 0x1) << 7) | (0x1 << 3) | (emr3 & 0x0077) | 0x0d00;
+	lpddr4_mr_write(3, 13, emr3);
+
+	/* 20. enter SR Power Down */
+	mmio_clrsetbits_32(DDRC_PWRCTL(0), 0x60, 0x20);
+
+	/* 21. Poll STAT.selfref_state is in "SR Power down" */
+	do {
+		val = mmio_read_32(DDRC_STAT(0));
+	} while ((val & 0x300) != 0x200);
+
+	/* 22. set dfi_init_complete_en = 0 */
+
+	/* 23. switch clock */
+	/* set SWCTL.dw_done to 0 */
+	mmio_write_32(DDRC_SWCTL(0), 0x0000);
+
+	/* 24. program frequency mode=1(bit 29), target_frequency=target_freq (bit 29) */
+	mmio_write_32(DDRC_MSTR2(0), fsp_index);
+
+	/* 25. DBICTL for FSP-OP[1], skip it if never enable it */
+
+	/* 26.trigger initialization in the PHY */
+
+	/* Q3: if refresh level is updated, then should program */
+	/* as updating refresh, need to toggle refresh_update_level signal */
+	val = mmio_read_32(DDRC_RFSHCTL3(0));
+	val = val ^ 0x2;
+	mmio_write_32(DDRC_RFSHCTL3(0), val);
+
+	/* Q4: only for legacy PHY, so here can skipped */
+
+	/* dfi_frequency -> 0x1x */
+	val = mmio_read_32(DDRC_DFIMISC(0));
+	val &= 0xFE;
+	val |= (fsp_index << 8);
+	mmio_write_32(DDRC_DFIMISC(0), val);
+	/* dfi_init_start */
+	val |= 0x20;
+	mmio_write_32(DDRC_DFIMISC(0), val);
+
+	/* polling dfi_init_complete de-assert */
+	do {
+		val = mmio_read_32(DDRC_DFISTAT(0));
+	} while ((val & 0x1) == 0x1);
+
+	/* change the clock frequency */
+	dram_clock_switch(info->timing_info->fsp_table[fsp_index], info->bypass_mode);
+
+	/* dfi_init_start de-assert */
+	mmio_clrbits_32(DDRC_DFIMISC(0), 0x20);
+
+	/* polling dfi_init_complete re-assert */
+	do {
+		val = mmio_read_32(DDRC_DFISTAT(0));
+	} while ((val & 0x1) == 0x0);
+
+	/* 27. set ZQCTL0.dis_srx_zqcl = 1 */
+	if (fsp_index == 0) {
+		mmio_setbits_32(DDRC_ZQCTL0(0), BIT(30));
+	} else  if (fsp_index == 1) {
+		mmio_setbits_32(DDRC_FREQ1_ZQCTL0(0), BIT(30));
+	} else {
+		mmio_setbits_32(DDRC_FREQ2_ZQCTL0(0), BIT(30));
+	}
+
+	/* 28,29. exit "self refresh power down" to stay "self refresh 2" */
+	/* exit SR power down */
+	mmio_clrsetbits_32(DDRC_PWRCTL(0), 0x60, 0x40);
+	/* 30. Poll STAT.selfref_state in "Self refresh 2" */
+	do {
+		val = mmio_read_32(DDRC_STAT(0));
+	} while ((val & 0x300) != 0x300);
+
+	/* 31. change MR13.VRCG to normal */
+	emr3 = (emr3 & 0x00f7) | 0x0d00;
+	lpddr4_mr_write(3, 13, emr3);
+
+	/* enable PHY master */
+	mmio_write_32(DDRC_DFIPHYMSTR(0), 0x1);
+
+	/* 32. issue ZQ if required: zq_calib_short, bit 4 */
+	/* polling zq_calib_short_busy */
+	mmio_setbits_32(DDRC_DBGCMD(0), 0x10);
+
+	do {
+		val = mmio_read_32(DDRC_DBGSTAT(0));
+	} while ((val & 0x10) != 0x0);
+
+	/* 33. Reset ZQCTL0.dis_srx_zqcl=0 */
+	if (fsp_index == 1)
+		mmio_clrbits_32(DDRC_FREQ1_ZQCTL0(0), BIT(30));
+	else if (fsp_index == 2)
+		mmio_clrbits_32(DDRC_FREQ2_ZQCTL0(0), BIT(30));
+	else
+		mmio_clrbits_32(DDRC_ZQCTL0(0), BIT(30));
+
+	/* set SWCTL.dw_done to 1 and poll SWSTAT.sw_done_ack=1 */
+	mmio_write_32(DDRC_SWCTL(0), 0x1);
+
+	/* wait SWSTAT.sw_done_ack to 1 */
+	do {
+		val = mmio_read_32(DDRC_SWSTAT(0));
+	} while ((val & 0x1) == 0x0);
+
+	/* 34. set PWRCTL.stay_in_selfreh=0, exit SR */
+	mmio_clrbits_32(DDRC_PWRCTL(0), 0x40);
+	/* wait tXSR */
+
+	/* 35. Poll STAT.selfref_state in "Idle" */
+	do {
+		val = mmio_read_32(DDRC_STAT(0));
+	} while ((val & 0x300) != 0x0);
+
+#ifdef DFILP_SPT
+	/* 36. restore dfi_lp.dfi_lp_en_sr */
+	mmio_setbits_32(DDRC_DFILPCFG0(0), BIT(8));
+#endif
+
+	/* 37. re-enable CAM: dis_dq */
+	mmio_clrbits_32(DDRC_DBG1(0), 0x1);
+
+	/* 38. re-enable automatic SR: selfref_en */
+	mmio_setbits_32(DDRC_PWRCTL(0), 0x1);
+
+	/* 39. re-enable automatic ZQ: dis_auto_zq=0 */
+	/* disable automatic ZQ calibration */
+	if (fsp_index == 1)
+		mmio_clrbits_32(DDRC_FREQ1_ZQCTL0(0), BIT(31));
+	else if (fsp_index == 2)
+		mmio_clrbits_32(DDRC_FREQ2_ZQCTL0(0), BIT(31));
+	else
+		mmio_clrbits_32(DDRC_ZQCTL0(0), BIT(31));
+	/* 40. re-emable automatic derating: derate_enable */
+	mmio_write_32(DDRC_DERATEEN(0), derate_backup[0]);
+	mmio_write_32(DDRC_FREQ1_DERATEEN(0), derate_backup[1]);
+	mmio_write_32(DDRC_FREQ2_DERATEEN(0), derate_backup[2]);
+
+	/* 41. write 1 to PCTRL.port_en */
+	mmio_write_32(DDRC_PCTRL_0(0), 0x1);
+
+	/* 42. enable SBRCTL.scrub_en, skip if never enable it */
+}
diff --git a/plat/imx/imx8m/imx8mm/include/platform_def.h b/plat/imx/imx8m/imx8mm/include/platform_def.h
index 32044e0f4..c181212f0 100644
--- a/plat/imx/imx8m/imx8mm/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mm/include/platform_def.h
@@ -6,6 +6,7 @@
 
 #include <arch.h>
 #include <common/tbbr/tbbr_img_def.h>
+#include <lib/utils_def.h>
 
 #define PLATFORM_LINKER_FORMAT		"elf64-littleaarch64"
 #define PLATFORM_LINKER_ARCH		aarch64
@@ -141,6 +142,7 @@
 #define GPR_TZASC_EN_LOCK		BIT(16)
 
 #define ANAMIX_MISC_CTL			U(0x124)
+#define DRAM_PLL_CTRL			(IMX_ANAMIX_BASE + 0x50)
 
 #define MAX_CSU_NUM			U(64)
 
diff --git a/plat/imx/imx8m/imx8mm/platform.mk b/plat/imx/imx8m/imx8mm/platform.mk
index df04997e8..721297549 100644
--- a/plat/imx/imx8m/imx8mm/platform.mk
+++ b/plat/imx/imx8m/imx8mm/platform.mk
@@ -20,7 +20,10 @@ include drivers/arm/gic/v3/gicv3.mk
 include lib/libfdt/libfdt.mk
 
 IMX_DRAM_SOURCES	:=	plat/imx/imx8m/ddr/dram.c		\
-				plat/imx/imx8m/ddr/dram_retention.c
+				plat/imx/imx8m/ddr/clock.c		\
+				plat/imx/imx8m/ddr/dram_retention.c	\
+				plat/imx/imx8m/ddr/ddr4_dvfs.c		\
+				plat/imx/imx8m/ddr/lpddr4_dvfs.c
 
 IMX_GIC_SOURCES		:=	${GICV3_SOURCES}			\
 				plat/common/plat_gicv3.c		\
diff --git a/plat/imx/imx8m/imx8mn/include/platform_def.h b/plat/imx/imx8m/imx8mn/include/platform_def.h
index 8d39ea6a5..8106229b4 100644
--- a/plat/imx/imx8m/imx8mn/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mn/include/platform_def.h
@@ -9,6 +9,8 @@
 #include <lib/utils_def.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
 
+#include <lib/utils_def.h>
+
 #define PLATFORM_LINKER_FORMAT		"elf64-littleaarch64"
 #define PLATFORM_LINKER_ARCH		aarch64
 
diff --git a/plat/imx/imx8m/imx8mn/platform.mk b/plat/imx/imx8m/imx8mn/platform.mk
index 5ffce44d2..0f3ad1a20 100644
--- a/plat/imx/imx8m/imx8mn/platform.mk
+++ b/plat/imx/imx8m/imx8mn/platform.mk
@@ -14,7 +14,10 @@ include lib/xlat_tables_v2/xlat_tables.mk
 include drivers/arm/gic/v3/gicv3.mk
 
 IMX_DRAM_SOURCES	:=	plat/imx/imx8m/ddr/dram.c		\
-				plat/imx/imx8m/ddr/dram_retention.c
+				plat/imx/imx8m/ddr/clock.c		\
+				plat/imx/imx8m/ddr/dram_retention.c	\
+				plat/imx/imx8m/ddr/ddr4_dvfs.c		\
+				plat/imx/imx8m/ddr/lpddr4_dvfs.c
 
 
 IMX_GIC_SOURCES		:=	${GICV3_SOURCES}			\
diff --git a/plat/imx/imx8m/imx8mp/platform.mk b/plat/imx/imx8m/imx8mp/platform.mk
index ded31ca68..45f29728b 100644
--- a/plat/imx/imx8m/imx8mp/platform.mk
+++ b/plat/imx/imx8m/imx8mp/platform.mk
@@ -16,7 +16,10 @@ include lib/xlat_tables_v2/xlat_tables.mk
 include drivers/arm/gic/v3/gicv3.mk
 
 IMX_DRAM_SOURCES	:=	plat/imx/imx8m/ddr/dram.c		\
-				plat/imx/imx8m/ddr/dram_retention.c
+				plat/imx/imx8m/ddr/clock.c		\
+				plat/imx/imx8m/ddr/dram_retention.c	\
+				plat/imx/imx8m/ddr/ddr4_dvfs.c		\
+				plat/imx/imx8m/ddr/lpddr4_dvfs.c
 
 IMX_GIC_SOURCES		:=	${GICV3_SOURCES}			\
 				plat/common/plat_gicv3.c		\
diff --git a/plat/imx/imx8m/include/dram.h b/plat/imx/imx8m/include/dram.h
index 3b81801bd..ad11a2724 100644
--- a/plat/imx/imx8m/include/dram.h
+++ b/plat/imx/imx8m/include/dram.h
@@ -50,9 +50,13 @@ struct dram_timing_info {
 struct dram_info {
 	int dram_type;
 	unsigned int num_rank;
+	uint32_t num_fsp;
 	int current_fsp;
 	int boot_fsp;
+	bool bypass_mode;
 	struct dram_timing_info *timing_info;
+	/* mr, emr, emr2, emr3, mr11, mr12, mr22, mr14 */
+	uint32_t mr_table[3][8];
 };
 
 extern struct dram_info dram_info;
@@ -65,4 +69,10 @@ void dram_phy_init(struct dram_timing_info *timing);
 void dram_enter_retention(void);
 void dram_exit_retention(void);
 
+void dram_clock_switch(unsigned int target_drate, bool bypass_mode);
+
+/* dram frequency change */
+void lpddr4_swffc(struct dram_info *info, unsigned int init_fsp, unsigned int fsp_index);
+void ddr4_swffc(struct dram_info *dram_info, unsigned int pstate);
+
 #endif /* DRAM_H */