1 files changed, 79 insertions, 0 deletions

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;
+}