plat: imx8mm: Add lpddr4 dvfs support

add LPDDR4 DVFS support on imx8mm.

Signed-off-by: Bai Ping <ping.bai@nxp.com>

9 files changed, 735 insertions, 3 deletions

diff --git a/plat/imx/common/imx8m/clock.c b/plat/imx/common/imx8m/clock.c
new file mode 100644
index 00000000..72ddbd9a
--- /dev/null
+++ b/plat/imx/common/imx8m/clock.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright 2018 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <debug.h>
+#include <mmio.h>
+#include <platform_def.h>
+
+#define CCM_IP_CLK_ROOT_GEN_TAGET(i)		(IMX_CCM_BASE + 0x80 * (i) + 0x00)
+#define CCM_IP_CLK_ROOT_GEN_TAGET_SET(i)	(IMX_CCM_BASE + 0x80 * (i) + 0x04)
+#define CCM_IP_CLK_ROOT_GEN_TAGET_CLR(i)	(IMX_CCM_BASE + 0x80 * (i) + 0x08)
+
+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(0x30389804, 1 << 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(0x30389804, 1 << 24);
+}
+
+void ddr_pll_bypass_dis(void)
+{
+	mmio_write_32(0x30389808, 1 << 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));
+}
+
+/* change the dram clock frequency */
+void dram_clock_switch(unsigned int target_freq)
+{
+	if(target_freq == 2) {  //25M
+		ddr_pll_bypass_100mts();
+	} else if (target_freq == 0x1) {
+		ddr_pll_bypass_400mts();
+	} else {
+		/* to reduce the dvfs latency. skip re-init pll */
+		ddr_pll_bypass_dis();
+	}
+}
diff --git a/plat/imx/common/imx8m/dram.c b/plat/imx/common/imx8m/dram.c
index 072fcbb4..26d464fe 100644
--- a/plat/imx/common/imx8m/dram.c
+++ b/plat/imx/common/imx8m/dram.c
@@ -8,9 +8,19 @@
 #include <ddrc.h>
 #include <dram.h>
 #include <mmio.h>
+#include <spinlock.h>
 
 static struct dram_info dram_info;
 
+/* lock used for DDR DVFS */
+spinlock_t dfs_lock;
+/* IRQ used for DDR DVFS */
+static uint32_t irqs_used[] = {74, 75, 76, 77};
+static volatile uint32_t wfe_done;
+static volatile bool wait_ddrc_hwffc_done = true;
+
+static unsigned int dev_fsp = 0x1;
+
 /* restore the ddrc config */
 void dram_umctl2_init(void)
 {
@@ -57,6 +67,7 @@ void dram_phy_init(void)
 	}
 }
 
+
 void dram_info_init(unsigned long dram_timing_base)
 {
 	uint32_t current_fsp, ddr_type;
@@ -82,6 +93,14 @@ void dram_info_init(unsigned long dram_timing_base)
 	 * we have done it in SPL stage and save in memory
 	 */
 	dram_info.timing_info = (struct dram_timing_info *)dram_timing_base;
+
+	/* switch to the highest frequency point */
+	if(current_fsp != 0x0) {
+		/* flush the L1/L2 cache */
+		dcsw_op_all(DCCSW);
+		lpddr4_swffc(dev_fsp, 0x0);
+		dev_fsp = (~dev_fsp) & 0x1;
+	}
 }
 
 void dram_enter_retention(void)
@@ -97,3 +116,63 @@ void dram_exit_retention(void)
 	if (dram_info.dram_type == DDRC_LPDDR4)
 		lpddr4_exit_retention();
 }
+
+int dram_dvfs_handler(uint32_t smc_fid,
+			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 target_freq = x1;
+	uint32_t online_cores = x2;
+
+	/* TODO add ddr4 dvfs support later */
+	if (dram_info.dram_type != DDRC_LPDDR4)
+		return 0;
+
+	if (target_freq == 0xf) {
+		/* set the WFE done status */
+		spin_lock(&dfs_lock);
+		wfe_done |= (1 << cpu_id * 8);
+		spin_unlock(&dfs_lock);
+
+		while (1) {
+			/* ddr frequency change done */
+			wfe();
+			if (!wait_ddrc_hwffc_done) {
+				break;
+			}
+		}
+	} else {
+		wait_ddrc_hwffc_done = true;
+		/* trigger the IRQ */
+		for (int i = 0; i < 4; i++) {
+			int irq = irqs_used[i] % 32;
+			if (cpu_id != i && (online_cores & (0x1 << (i * 8)))) {
+				mmio_write_32(0x38800204 + (irqs_used[i] / 32) * 4, (1 << irq));
+			}
+		}
+
+		/* 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);
+
+		lpddr4_swffc(dev_fsp, target_freq);
+		dev_fsp = (~dev_fsp) & 0x1;
+
+		wait_ddrc_hwffc_done = false;
+		wfe_done = 0;
+		dsb();
+		sev();
+		isb();
+	}
+
+	return 0;
+}
diff --git a/plat/imx/common/imx8m/lpddr4_define.h b/plat/imx/common/imx8m/lpddr4_define.h
new file mode 100644
index 00000000..4b1153e4
--- /dev/null
+++ b/plat/imx/common/imx8m/lpddr4_define.h
@@ -0,0 +1,106 @@
+/*
+ * Copyright 2018 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef __LPDDR4_DEFINE_H__
+#define __LPDDR4_DEFINE_H__
+
+//----------------------------------------------------------------
+//     DRAM MR setting
+//----------------------------------------------------------------
+//#define LPDDR4_FSP0_MR1 0xD4
+//#define LPDDR4_FSP0_MR2 0x2D
+//#define LPDDR4_FSP0_MR3 0x31
+#define DDR_ONE_RANK
+
+
+#ifdef LPDDR4_DBI_ON
+#define LPDDR4_MR3			0xf1
+#define LPDDR4_PHY_DMIPinPresent	0x1 
+#else
+#define LPDDR4_MR3			0x31
+#define LPDDR4_PHY_DMIPinPresent	0x0 
+#endif
+
+#ifdef DDR_ONE_RANK
+#define LPDDR4_CS			0x1
+#else
+#define LPDDR4_CS			0x3
+#endif
+
+#define LPDDR4_HDT_CTL_2D		0xC8
+#define LPDDR4_HDT_CTL_3200_1D		0xC8
+#define LPDDR4_HDT_CTL_400_1D		0xC8
+#define LPDDR4_HDT_CTL_100_1D		0xC8
+
+#define LPDDR4_HDT_CTL_2D		0xC8
+#define LPDDR4_HDT_CTL_3200_1D		0xC8
+#define LPDDR4_HDT_CTL_400_1D		0xC8
+#define LPDDR4_HDT_CTL_100_1D		0xC8
+
+#ifdef RUN_ON_SILICON
+/* 400/100 training seq */
+#define LPDDR4_TRAIN_SEQ_P2		0x121f
+#define LPDDR4_TRAIN_SEQ_P1		0x121f
+#define LPDDR4_TRAIN_SEQ_P0		0x121f
+#else
+#define LPDDR4_TRAIN_SEQ_P2		 0x7
+#define LPDDR4_TRAIN_SEQ_P1 		0x7
+#define LPDDR4_TRAIN_SEQ_P0 		0x7
+#endif
+
+/* 2D share & weight */
+#define LPDDR4_2D_WEIGHT 		0x1f7f
+#define LPDDR4_2D_SHARE 		1
+#define LPDDR4_CATRAIN_3200_1d		0
+#define LPDDR4_CATRAIN_400		0
+#define LPDDR4_CATRAIN_100		0
+#define LPDDR4_CATRAIN_3200_2d		0
+
+/* MRS parameter */
+
+/* for LPDDR4 Rtt */
+#define LPDDR4_RTT40		6
+#define LPDDR4_RTT48		5
+#define LPDDR4_RTT60		4
+#define LPDDR4_RTT80		3
+#define LPDDR4_RTT120		2
+#define LPDDR4_RTT240		1
+#define LPDDR4_RTT_DIS		0
+
+/* for LPDDR4 Ron */
+#define LPDDR4_RON34		7
+#define LPDDR4_RON40		6
+#define LPDDR4_RON48		5
+#define LPDDR4_RON60		4
+#define LPDDR4_RON80		3
+
+#define LPDDR4_PHY_ADDR_RON60		0x1
+#define LPDDR4_PHY_ADDR_RON40		0x3
+#define LPDDR4_PHY_ADDR_RON30		0x7
+#define LPDDR4_PHY_ADDR_RON24		0xf
+#define LPDDR4_PHY_ADDR_RON20		0x1f
+
+/* for read channel */
+#define LPDDR4_RON			LPDDR4_RON40
+#define LPDDR4_PHY_RTT			30
+#define LPDDR4_PHY_VREF_VALUE 		17
+
+/* for write channel */
+#define LPDDR4_PHY_RON			30
+#define LPDDR4_PHY_ADDR_RON		LPDDR4_PHY_ADDR_RON40
+#define LPDDR4_RTT_DQ			LPDDR4_RTT40
+#define LPDDR4_RTT_CA			LPDDR4_RTT40
+#define LPDDR4_RTT_CA_BANK0		LPDDR4_RTT40
+#define LPDDR4_RTT_CA_BANK1		LPDDR4_RTT40
+#define LPDDR4_VREF_VALUE_CA		((1<<6)|(0xd))
+#define LPDDR4_VREF_VALUE_DQ_RANK0	((1<<6)|(0xd))
+#define LPDDR4_VREF_VALUE_DQ_RANK1	((1<<6)|(0xd))
+#define LPDDR4_MR22_RANK0		((0<<5)|(0<<4)|(0<<3)|(LPDDR4_RTT40))
+#define LPDDR4_MR22_RANK1		((1<<5)|(0<<4)|(1<<3)|(LPDDR4_RTT40))
+
+#define LPDDR4_MR3_PU_CAL		1
+
+#endif /*__LPDDR4_DEFINE_H__ */
diff --git a/plat/imx/common/imx8m/lpddr4_dvfs.c b/plat/imx/common/imx8m/lpddr4_dvfs.c
new file mode 100644
index 00000000..bfc39f50
--- /dev/null
+++ b/plat/imx/common/imx8m/lpddr4_dvfs.c
@@ -0,0 +1,365 @@
+/*
+ * Copyright 2018 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <debug.h>
+#include <ddrc.h>
+#include <dram.h>
+#include <mmio.h>
+
+#include "lpddr4_define.h"
+
+#define P0_INIT3 0x00D4002D
+#define P0_INIT4 ((LPDDR4_MR3 << 16) | 0x0000)
+#define P0_INIT6 0x0066004a
+#define P0_INIT7 0x0006004a
+
+#define P1_INIT3 0x00840000
+#define P1_INIT4 ((LPDDR4_MR3 << 16) | 0x0000)
+#define P1_INIT6 0x0066004a
+#define P1_INIT7 0x0006004a
+
+#define P2_INIT3 0x00840000
+#define P2_INIT4 ((LPDDR4_MR3 << 16) | 0x0000)
+#define P2_INIT6 0x0066004a
+#define P2_INIT7 0x0006004a
+
+extern void dram_clock_switch(unsigned target_freq);
+
+void lpddr4_swffc(unsigned int init_fsp, unsigned int tgt_freq)
+
+{
+	unsigned int mr, emr, emr2, emr3;
+	unsigned int mr11, mr12, mr22, mr14;
+	unsigned int tmp;
+
+	/* 1. program targetd UMCTL2_REGS_FREQ1/2/3,already done, skip it. */
+
+	/* 2. MR13.FSP-WR=1, MRW to update MR registers */
+	if (tgt_freq == 0) {
+		mr   = P0_INIT3 >> 16;
+		emr  = P0_INIT3 & 0xFFFF;
+		emr2 = P0_INIT4 >> 16;
+		emr3 = P0_INIT4 & 0xFFFF;
+		mr11 = P0_INIT6 >> 16;
+		mr12 = P0_INIT6 & 0xFFFF;
+		mr22 = P0_INIT7 >> 16;
+		mr14 = P0_INIT7 & 0xFFFF;
+	} else if (tgt_freq == 1) {
+		mr   = P1_INIT3 >> 16;
+		emr  = P1_INIT3 & 0xFFFF;
+		emr2 = P1_INIT4 >> 16;
+		emr3 = P1_INIT4 & 0xFFFF;
+		mr11 = P1_INIT6 >> 16;
+		mr12 = P1_INIT6 & 0xFFFF;
+		mr22 = P1_INIT7 >> 16;
+		mr14 = P1_INIT7 & 0xFFFF;
+	} else {
+		mr   = P2_INIT3 >> 16;
+		emr  = P2_INIT3 & 0xFFFF;
+		emr2 = P2_INIT4 >> 16;
+		emr3 = P2_INIT4 & 0xFFFF;
+		mr11 = P2_INIT6 >> 16;
+		mr12 = P2_INIT6 & 0xFFFF;
+		mr22 = P2_INIT7 >> 16;
+		mr14 = P2_INIT7 & 0xFFFF;
+	}
+
+	tmp = (init_fsp == 1) ? 0x2 << 6 : 0x1 << 6;
+	emr3 = (emr3 & 0x003f) | tmp | 0x0d00;
+
+	/* 12. set PWRCTL.selfref_en=0 */
+	tmp= mmio_read_32(DDRC_PWRCTL(0));
+	tmp &= ~0xf; 
+	mmio_write_32(DDRC_PWRCTL(0), tmp);
+
+	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 {
+		tmp = mmio_read_32(DDRC_MRSTAT(0));
+	} while (tmp & 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 {
+		tmp = mmio_read_32(DDRC_PSTAT(0));
+	} while (tmp != 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 */
+	mmio_write_32(DDRC_DFIPHYMSTR(0),0x00000000);
+
+#ifdef DFILP_SPT 
+	/* 8. disable DFI LP */
+	/* DFILPCFG0.dfi_lp_en_sr */
+	tmp = mmio_read_32(DDRC_DFILPCFG0(0));
+	if (tmp & 0x100) { 
+		mmio_write_32(DDRC_DFILPCFG0(0), 0x0);
+		do {
+			tmp = mmio_read_32(DDRC_DFISTAT(0)); // dfi_lp_ack
+			tmp2= mmio_read_32(DDRC_STAT(0)); // operating_mode
+		} while (((tmp & 0x2) == 0x2) && ((tmp2 & 0x7) == 3));
+	}
+#endif
+	/* 9. wait until in normal or power down states */
+	do {
+		/* operating_mode */
+		tmp= mmio_read_32(DDRC_STAT(0));
+	} while (((tmp & 0x7) != 1) && ((tmp & 0x7) != 2));
+
+	/* 10. Disable automatic derating: derate_enable */
+	tmp= mmio_read_32(DDRC_DERATEEN(0));
+	tmp &= ~0x1;
+	mmio_write_32(DDRC_DERATEEN(0), tmp);
+
+	tmp= mmio_read_32(DDRC_FREQ1_DERATEEN(0));
+	tmp &= ~0x1;
+	mmio_write_32(DDRC_FREQ1_DERATEEN(0), tmp);
+
+	tmp= mmio_read_32(DDRC_FREQ2_DERATEEN(0));
+	tmp &= ~0x1;
+	mmio_write_32(DDRC_FREQ2_DERATEEN(0), tmp);
+
+	/* 11. disable automatic ZQ calibration */
+	tmp= mmio_read_32(DDRC_ZQCTL0(0));
+	tmp |= 0x80000000;
+	mmio_write_32(DDRC_ZQCTL0(0), tmp);
+
+	tmp= mmio_read_32(DDRC_FREQ1_ZQCTL0(0));
+	tmp |= 0x80000000;
+	mmio_write_32(DDRC_FREQ1_ZQCTL0(0), tmp);
+
+	tmp= mmio_read_32(DDRC_FREQ2_ZQCTL0(0));
+	tmp |= 0x80000000;
+	mmio_write_32(DDRC_FREQ2_ZQCTL0(0), tmp);
+
+	/* 12. set PWRCTL.selfref_en=0 */
+	tmp= mmio_read_32(DDRC_PWRCTL(0));
+	tmp &= ~0x1; 
+	mmio_write_32(DDRC_PWRCTL(0), tmp);
+
+	/* 13.Poll STAT.operating_mode is in "Normal" (001) or "Power-down" (010) */
+	do {
+		/* operating_mode */
+		tmp= mmio_read_32(DDRC_STAT(0));
+	} while (((tmp & 0x7) != 1) && ((tmp & 0x7) != 2));
+
+	/* 14-15. trigger SW SR */
+	tmp= mmio_read_32(DDRC_PWRCTL(0));
+ 	/* bit 5: selfref_sw, bit 6: stay_in_selfref */
+	tmp |= 0x60;
+	mmio_write_32(DDRC_PWRCTL(0),tmp);
+
+	/* 16. Poll STAT.selfref_state in "Self Refresh 1" */
+	do {
+		tmp= mmio_read_32(DDRC_STAT(0));
+	} while ((tmp & 0x300) != 0x100);
+
+	/* 17. disable dq */
+	tmp= mmio_read_32(DDRC_DBG1(0));
+	tmp |= 0x1;
+	mmio_write_32(DDRC_DBG1(0), tmp);
+
+	/* 18. Poll DBGCAM.wr_data_pipeline_empty and DBGCAM.rd_data_pipeline_empty */
+	do {
+		tmp  = mmio_read_32(DDRC_DBGCAM(0));
+		tmp &= 0x30000000;
+	} while (tmp != 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 */
+	tmp= mmio_read_32(DDRC_PWRCTL(0));
+	tmp &= ~0x60;
+	tmp |= 0x20;
+	mmio_write_32(DDRC_PWRCTL(0),tmp);
+
+	/* 21. Poll STAT.selfref_state is in "SR Power down" */
+	do {
+		tmp= mmio_read_32(DDRC_STAT(0)); 
+	} while ((tmp & 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), tgt_freq);
+
+	/* 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 */
+	tmp= mmio_read_32(DDRC_RFSHCTL3(0));
+	tmp = tmp ^ 0x2;
+	mmio_write_32(DDRC_RFSHCTL3(0), tmp);
+
+	/* Q4: only for legacy PHY, so here can skipped */
+
+	/* dfi_frequency -> 0x1x */
+	tmp= mmio_read_32(DDRC_DFIMISC(0));
+	tmp &= 0xFE; 
+	tmp |= (tgt_freq << 8);
+	mmio_write_32(DDRC_DFIMISC(0), tmp);
+	/* dfi_init_start */
+	tmp |= 0x20;
+	mmio_write_32(DDRC_DFIMISC(0), tmp);
+
+	/* polling dfi_init_complete de-assert */
+	do {
+		tmp = mmio_read_32(DDRC_DFISTAT(0));
+	} while ((tmp & 0x1) == 0x1);
+
+	/* change the clock frequency */
+	dram_clock_switch(tgt_freq);
+
+	/* dfi_init_start de-assert */
+	tmp= mmio_read_32(DDRC_DFIMISC(0));
+	tmp &= ~0x20;
+	mmio_write_32(DDRC_DFIMISC(0),tmp);
+
+	/* polling dfi_init_complete re-assert */
+	do {
+		tmp = mmio_read_32(DDRC_DFISTAT(0));
+	} while ((tmp & 0x1) == 0x0);
+  
+	/* 27. set ZQCTL0.dis_srx_zqcl = 1 */
+	if (tgt_freq == 0) {
+		tmp = mmio_read_32(DDRC_ZQCTL0(0));
+		tmp |= 0x40000000;
+		mmio_write_32(DDRC_ZQCTL0(0), tmp);
+	} else  if (tgt_freq == 1) {
+		tmp = mmio_read_32(DDRC_FREQ1_ZQCTL0(0));
+		tmp |= 0x40000000;
+		mmio_write_32(DDRC_FREQ1_ZQCTL0(0), tmp);
+	} else {
+		tmp = mmio_read_32(DDRC_FREQ2_ZQCTL0(0));
+		tmp |= 0x40000000;
+		mmio_write_32(DDRC_FREQ2_ZQCTL0(0), tmp);
+	}
+
+	/* 28,29. exit "self refresh power down" to stay "self refresh 2" */
+	/* exit SR power down */
+	tmp= mmio_read_32(DDRC_PWRCTL(0));
+	tmp &= ~(0x60);
+	tmp |= 0x40; 
+	mmio_write_32(DDRC_PWRCTL(0), tmp);
+	/* 30. Poll STAT.selfref_state in "Self refresh 2" */
+	do {
+		tmp= mmio_read_32(DDRC_STAT(0));
+	} while ((tmp & 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 */
+	tmp= mmio_read_32(DDRC_DBGCMD(0));
+	tmp |= 0x10; 
+	mmio_write_32(DDRC_DBGCMD(0), tmp);
+
+	do {
+		tmp= mmio_read_32(DDRC_DBGSTAT(0));
+	} while ((tmp & 0x10 ) != 0x0);
+
+#if 1
+	/* 33. Reset ZQCTL0.dis_srx_zqcl=0 */
+	if (tgt_freq == 1) {
+		tmp = mmio_read_32(DDRC_FREQ1_ZQCTL0(0));
+		tmp &= ~0x40000000;
+		mmio_write_32(DDRC_FREQ1_ZQCTL0(0), tmp);
+	}  else if (tgt_freq == 2) {
+		tmp = mmio_read_32(DDRC_FREQ2_ZQCTL0(0));
+		tmp &= ~0x40000000;
+		mmio_write_32(DDRC_FREQ2_ZQCTL0(0), tmp);
+	} else {
+		tmp = mmio_read_32(DDRC_ZQCTL0(0));
+		tmp &= ~0x40000000;
+		mmio_write_32(DDRC_ZQCTL0(0), tmp);
+	}
+
+	/* set SWCTL.dw_done to 1 and poll SWSTAT.sw_done_ack=1 */
+	mmio_write_32(DDRC_SWCTL(0), 0x0001); 
+
+	/* wait SWSTAT.sw_done_ack to 1 */
+	do {
+		tmp = mmio_read_32(DDRC_SWSTAT(0));
+	} while ((tmp & 0x1) == 0x0);
+
+	/* 34. set PWRCTL.stay_in_selfreh=0, exit SR */
+	tmp= mmio_read_32(DDRC_PWRCTL(0));
+	tmp &= ~(0x40);
+	mmio_write_32(DDRC_PWRCTL(0),tmp);
+	/* wait tXSR */
+
+	/* 35. Poll STAT.selfref_state in "Idle" */
+	do {
+		tmp = mmio_read_32(DDRC_STAT(0));
+	} while ((tmp & 0x300) != 0x0);
+
+#ifdef DFILP_SPT 
+	/* 36. restore dfi_lp.dfi_lp_en_sr */
+	tmp = mmio_read_32(DDRC_DFILPCFG0(0));
+	tmp |= 0x100;
+	mmio_write_32(DDRC_DFILPCFG0(0),tmp);
+#endif
+
+	/* 37. re-enable CAM: dis_dq */
+	tmp= mmio_read_32(DDRC_DBG1(0));
+	tmp &= 0xFFFFFFFE;
+	mmio_write_32(DDRC_DBG1(0), tmp);
+
+	/* 38. re-enable automatic SR: selfref_en */
+	tmp= mmio_read_32(DDRC_PWRCTL(0));
+	tmp |= 0x1;
+	mmio_write_32(DDRC_PWRCTL(0), tmp);
+
+	/* 39. re-enable automatic ZQ: dis_auto_zq=0 */
+	/* disable automatic ZQ calibration */
+	if (tgt_freq == 1) {
+		tmp = mmio_read_32(DDRC_FREQ1_ZQCTL0(0));
+		tmp &= ~0x80000000;
+		mmio_write_32(DDRC_FREQ1_ZQCTL0(0), tmp);
+	} else if (tgt_freq == 2) {
+		tmp = mmio_read_32(DDRC_FREQ2_ZQCTL0(0));
+		tmp &= ~0x80000000;
+		mmio_write_32(DDRC_FREQ2_ZQCTL0(0), tmp);
+	} else {
+		tmp = mmio_read_32(DDRC_ZQCTL0(0));
+		tmp &= ~0x80000000;
+		mmio_write_32(DDRC_ZQCTL0(0), tmp);
+	}
+
+	/* 40. re-emable automatic derating: derate_enable */
+	tmp= mmio_read_32(DDRC_DERATEEN(0));
+	tmp &= 0xFFFFFFFE;
+	mmio_write_32(DDRC_DERATEEN(0), tmp);
+
+	/* 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 */
+#endif
+}
diff --git a/plat/imx/common/imx8m/lpddr4_helper.c b/plat/imx/common/imx8m/lpddr4_helper.c
new file mode 100644
index 00000000..e9446aa0
--- /dev/null
+++ b/plat/imx/common/imx8m/lpddr4_helper.c
@@ -0,0 +1,56 @@
+/*
+ * Copyright 2018 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <debug.h>
+#include <ddrc.h>
+#include <dram.h>
+#include <mmio.h>
+
+void lpddr4_mr_write(uint32_t mr_rank, uint32_t mr_addr, uint32_t mr_data)
+{
+	uint32_t tmp;
+	/*
+	 * 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.
+	 */
+	do {
+		tmp = mmio_read_32(DDRC_MRSTAT(0));
+	} while(tmp & 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), (1 << 31));
+}
+
+uint32_t lpddr4_mr_read(uint32_t mr_rank, uint32_t mr_addr)
+{
+	uint32_t tmp, mr_data;
+
+	mmio_write_32(DRC_PERF_MON_MRR0_DAT(0), 0x1);
+	do {
+		tmp = mmio_read_32(DDRC_MRSTAT(0));
+	} while(tmp & 0x1);
+
+	mmio_write_32(DDRC_MRCTRL0(0), (mr_rank << 4) | 0x1);
+	mmio_write_32(DDRC_MRCTRL1(0), (mr_addr << 8));
+	mmio_setbits_32(DDRC_MRCTRL0(0), (1 << 31));
+
+	do {
+		tmp = mmio_read_32(DRC_PERF_MON_MRR0_DAT(0));
+	} while((tmp & 0x8) == 0);
+
+	tmp = mmio_read_32(DRC_PERF_MON_MRR1_DAT(0));
+	mr_data = tmp & 0xff;
+	mmio_write_32(DRC_PERF_MON_MRR0_DAT(0), 0x4);
+
+	return mr_data;
+}
diff --git a/plat/imx/common/include/dram.h b/plat/imx/common/include/dram.h
index fd0130c3..fd606f57 100644
--- a/plat/imx/common/include/dram.h
+++ b/plat/imx/common/include/dram.h
@@ -8,6 +8,24 @@
 #define __DRAM_H__
 
 #include <utils_def.h>
+#include <arch_helpers.h>
+#include <assert.h>
+#include <bl_common.h>
+#include <console.h>
+#include <context.h>
+#include <context_mgmt.h>
+#include <debug.h>
+#include <stdbool.h>
+#include <mmio.h>
+#include <platform.h>
+#include <platform_def.h>
+#include <plat_imx8.h>
+#include <xlat_tables.h>
+#include <soc.h>
+#include <tzc380.h>
+#include <imx_csu.h>
+#include <imx_rdc.h>
+#include <uart.h>
 
 #define DDRC_LPDDR4	BIT(5)
 #define DDR_TYPE_MASK	0x3f
@@ -45,13 +63,23 @@ struct dram_info {
 	struct dram_timing_info *timing_info;
 };
 
+void lpddr4_mr_write(uint32_t, uint32_t, uint32_t);
+uint32_t lpddr4_mr_read(uint32_t, uint32_t);
+
 void ddrphy_load_pie_image(void);
 void dram_info_init(unsigned long dram_timing_base);
-void lpddr4_enter_retention(void);
-void lpddr4_exit_retention(void);
 void dram_umctl2_init(void);
 void dram_phy_init(void);
+
+void lpddr4_enter_retention(void);
+void lpddr4_exit_retention(void);
+/* lpddr4 swffc for dvfs */
+void lpddr4_swffc(unsigned int dev_fsp, unsigned int tgt_freq);
+
+/* dram retention */
 void dram_enter_retention(void);
 void dram_exit_retention(void);
 
+void dram_clock_switch(unsigned int target_freq);
+
 #endif /* __DRAM_H__ */
diff --git a/plat/imx/common/sip_svc.c b/plat/imx/common/sip_svc.c
index 3899f2d9..596da8c3 100644
--- a/plat/imx/common/sip_svc.c
+++ b/plat/imx/common/sip_svc.c
@@ -26,6 +26,7 @@ extern int imx_src_handler(uint32_t  smc_fid, u_register_t x1, u_register_t x2,
 extern int imx_soc_handler(uint32_t smc_fid, u_register_t x1, u_register_t x2, u_register_t x3);
 extern int imx_hab_handler(uint32_t smc_fid, u_register_t x1, u_register_t x2, u_register_t x3, u_register_t x4);
 extern int imx_noc_handler(uint32_t smc_fid, u_register_t x1, u_register_t x2, u_register_t x3);
+extern int dram_dvfs_handler(uint32_t smc_fid, u_register_t x1, u_register_t x2, u_register_t x3);
 
 bool wakeup_src_irqsteer = false;
 
@@ -119,6 +120,11 @@ uintptr_t imx_svc_smc_handler(uint32_t smc_fid,
 		SMC_RET1(handle, lpddr4_dvfs_handler(smc_fid, x1, x2, x3));
 		break;
 #endif
+#if defined(PLAT_IMX8MM)
+	case FSL_SIP_DDR_DVFS:
+		SMC_RET1(handle, dram_dvfs_handler(smc_fid, x1, x2, x3));
+		break;
+#endif
 #if defined(PLAT_IMX8M) || defined(PLAT_IMX8MM)
 	case  FSL_SIP_GPC:
 		SMC_RET1(handle, imx_gpc_handler(smc_fid, x1, x2, x3));
diff --git a/plat/imx/imx8mm/include/ddrc.h b/plat/imx/imx8mm/include/ddrc.h
index 8eb42fd8..ed6ebfa1 100644
--- a/plat/imx/imx8mm/include/ddrc.h
+++ b/plat/imx/imx8mm/include/ddrc.h
@@ -297,6 +297,33 @@
 #define DDRC_DFITMG3_SHADOW(X)         (DDRC_IPS_BASE_ADDR(X) + 0x21b8)
 #define DDRC_ODTCFG_SHADOW(X)          (DDRC_IPS_BASE_ADDR(X) + 0x2240)
 
+#define DRC_PERF_MON_BASE_ADDR(X)   	0x3d800000 + (X * 0x2000000)
+#define DRC_PERF_MON_CNT0_CTL(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x0
+#define DRC_PERF_MON_CNT1_CTL(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x4
+#define DRC_PERF_MON_CNT2_CTL(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x8
+#define DRC_PERF_MON_CNT3_CTL(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0xC
+#define DRC_PERF_MON_CNT0_DAT(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x20
+#define DRC_PERF_MON_CNT1_DAT(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x24
+#define DRC_PERF_MON_CNT2_DAT(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x28
+#define DRC_PERF_MON_CNT3_DAT(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x2C
+#define DRC_PERF_MON_DPCR_DAT(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x30
+#define DRC_PERF_MON_MRR0_DAT(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x40
+#define DRC_PERF_MON_MRR1_DAT(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x44
+#define DRC_PERF_MON_MRR2_DAT(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x48
+#define DRC_PERF_MON_MRR3_DAT(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x4C
+#define DRC_PERF_MON_MRR4_DAT(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x50
+#define DRC_PERF_MON_MRR5_DAT(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x54
+#define DRC_PERF_MON_MRR6_DAT(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x58
+#define DRC_PERF_MON_MRR7_DAT(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x5C
+#define DRC_PERF_MON_MRR8_DAT(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x60
+#define DRC_PERF_MON_MRR9_DAT(X) 	DRC_PERF_MON_BASE_ADDR(X) + 0x64
+#define DRC_PERF_MON_MRR10_DAT(X)	DRC_PERF_MON_BASE_ADDR(X) + 0x68
+#define DRC_PERF_MON_MRR11_DAT(X)	DRC_PERF_MON_BASE_ADDR(X) + 0x6C
+#define DRC_PERF_MON_MRR12_DAT(X)	DRC_PERF_MON_BASE_ADDR(X) + 0x70
+#define DRC_PERF_MON_MRR13_DAT(X)	DRC_PERF_MON_BASE_ADDR(X) + 0x74
+#define DRC_PERF_MON_MRR14_DAT(X)	DRC_PERF_MON_BASE_ADDR(X) + 0x78
+#define DRC_PERF_MON_MRR15_DAT(X)	DRC_PERF_MON_BASE_ADDR(X) + 0x7C
+
 #define IP2APB_DDRPHY_IPS_BASE_ADDR(X) (0x3c000000 + (X * 0x2000000))
 #define dwc_ddrphy_apb_rd(addr) mmio_read_32(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * (addr))
 #define dwc_ddrphy_apb_wr(addr, val) mmio_write_32(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * (addr), val)
diff --git a/plat/imx/imx8mm/platform.mk b/plat/imx/imx8mm/platform.mk
index 0cbeb162..a38b99c1 100644
--- a/plat/imx/imx8mm/platform.mk
+++ b/plat/imx/imx8mm/platform.mk
@@ -10,7 +10,10 @@ PLAT_GIC_SOURCES	:=	drivers/arm/gic/v3/gicv3_helpers.c	\
 				plat/imx/common/plat_imx8_gic.c
 
 PLAT_DRAM_SOURCES	:=	plat/imx/common/imx8m/dram.c		\
-				plat/imx/common/imx8m/lpddr4_retention.c
+				plat/imx/common/imx8m/clock.c		\
+				plat/imx/common/imx8m/lpddr4_retention.c \
+				plat/imx/common/imx8m/lpddr4_helper.c	\
+				plat/imx/common/imx8m/lpddr4_dvfs.c
 
 BL31_SOURCES		+=	plat/imx/common/imx8_helpers.S		\
 				plat/imx/common/mxcuart_console.S	\