/*
 * Copyright 2021-2024 NXP
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <assert.h>
#include <stdbool.h>

#include <arch_helpers.h>
#include <bl31/interrupt_mgmt.h>
#include <common/runtime_svc.h>
#include <lib/mmio.h>
#include <lib/spinlock.h>
#include <plat/common/platform.h>

#include <platform_def.h>

#include <dram.h>
#include <upower_api.h>

#define PHY_FREQ_SEL_INDEX(x)		((x) << 16)
#define PHY_FREQ_MULTICAST_EN(x)	((x) << 8)
#define DENALI_PHY_1537			U(0x5804)

#define IMX_DDRC_BASE			U(0x2E060000)
#define SAVED_DRAM_DATA_BASE		U(0x20055000)
#define DENALI_CTL_144			0x240
#define LPI_WAKEUP_EN_SHIFT		U(8)
#define IMX_LPAV_SIM_BASE		0x2DA50000
#define LPDDR_CTRL			0x14
#define LPDDR_AUTO_LP_MODE_DISABLE	BIT(24)
#define SOC_LP_CMD_SHIFT		U(15)
#define LPDDR_CTRL2			0x18

#define DENALI_CTL_00			U(0x0)
#define DENALI_CTL_23			U(0x5c)
#define DFIBUS_FREQ_INIT_SHIFT		U(24)
#define TSREF2PHYMSTR_SHIFT		U(8)
#define TSREF2PHYMSTR_MASK		GENMASK(13, 8)

#define DENALI_CTL_24			U(0x60)
#define DENALI_CTL_25			U(0x64)

#define DENALI_CTL_93			U(0x174)
#define PWRUP_SREFRESH_EXIT		BIT(0)

#define DENALI_CTL_127				U(0x1fc)
#define PHYMSTR_TRAIN_AFTER_INIT_COMPLETE	BIT(16)

#define DENALI_CTL_147			U(0x24c)
#define DENALI_CTL_153			U(0x264)
#define PCPCS_PD_EN			BIT(8)

#define DENALI_CTL_249			U(0x3E4)
#define DENALI_CTL_266			U(0x428)

#define DENALI_PHY_1547			U(0x582c)
#define PHY_LP4_BOOT_DISABLE		BIT(8)

#define DENALI_PHY_1559			U(0x585c)
#define DENALI_PHY_1590			U(0x58D8)

#define DENALI_PI_00			U(0x2000)
#define DENALI_PI_04			U(0x2010)
#define DENALI_PI_52			U(0x20D0)
#define DENALI_PI_26			U(0x2068)
#define DENALI_PI_33			U(0x2084)
#define DENALI_PI_65			U(0x2104)
#define DENALI_PI_77			U(0x2134)
#define DENALI_PI_134			U(0x2218)
#define DENALI_PI_131			U(0x220C)
#define DENALI_PI_132			U(0x2210)
#define DENALI_PI_134			U(0x2218)
#define DENALI_PI_137			U(0x2224)
#define DENALI_PI_174			U(0x22B8)
#define DENALI_PI_175			U(0x22BC)
#define DENALI_PI_181			U(0x22D4)
#define DENALI_PI_182			U(0x22D8)
#define DENALI_PI_191			U(0x22FC)
#define DENALI_PI_192			U(0x2300)
#define DENALI_PI_212			U(0x2350)
#define DENALI_PI_214			U(0x2358)
#define DENALI_PI_217			U(0x2364)

#define LPDDR3_TYPE	U(0x7)
#define LPDDR4_TYPE	U(0xB)

extern void upower_wait_resp(void);

struct dram_cfg_param {
	uint32_t reg;
	uint32_t val;
};

struct dram_timing_info {
	/* ddr controller config */
	struct dram_cfg_param *ctl_cfg;
	unsigned int ctl_cfg_num;
	/* pi config */
	struct dram_cfg_param *pi_cfg;
	unsigned int pi_cfg_num;
	/* phy freq1 config */
	struct dram_cfg_param *phy_f1_cfg;
	unsigned int phy_f1_cfg_num;
	/* phy freq2 config */
	struct dram_cfg_param *phy_f2_cfg;
	unsigned int phy_f2_cfg_num;
	/* initialized drate table */
	unsigned int fsp_table[3];
};

#define CTL_NUM		U(680)
#define PI_NUM		U(298)
#define PHY_NUM		U(1654)
#define PHY_DIFF_NUM	U(49)
struct dram_cfg {
	uint32_t ctl_cfg[CTL_NUM];
	uint32_t pi_cfg[PI_NUM];
	uint32_t phy_full[PHY_NUM];
	uint32_t phy_diff[PHY_DIFF_NUM];
};

struct dram_timing_info *info;
struct dram_cfg *dram_timing_cfg;

/* mark if dram cfg is already saved */
static bool dram_cfg_saved;
static uint32_t dram_class;

/* PHY register index for frequency diff */
uint32_t freq_specific_reg_array[PHY_DIFF_NUM] = {
90, 92, 93, 96, 97, 100, 101, 102, 103, 104, 114,
346, 348, 349, 352, 353, 356, 357, 358, 359, 360,
370, 602, 604, 605, 608, 609, 612, 613, 614, 615,
616, 626, 858, 860, 861, 864, 865, 868, 869, 870,
871, 872, 882, 1063, 1319, 1566, 1624, 1625
};

/* lock used for DDR DVFS */
spinlock_t dfs_lock;
static volatile uint32_t core_count;
static volatile bool in_progress;
static volatile bool sys_dvfs;
static int num_fsp;

static void ddr_init(void)
{
	unsigned int i;

	/* restore the ddr ctl config */
	for (i = 0U; i < CTL_NUM; i++) {
		mmio_write_32(IMX_DDRC_BASE + i * 4, dram_timing_cfg->ctl_cfg[i]);
	}

	/* load the PI registers */
	for (i = 0U; i < PI_NUM; i++) {
		mmio_write_32(IMX_DDRC_BASE + 0x2000 + i * 4, dram_timing_cfg->pi_cfg[i]);
	}


	 /* restore all PHY registers for all the fsp. */
	mmio_write_32(IMX_DDRC_BASE + DENALI_PHY_1537, 0x100);
	/* restore all the phy configs */
	for (i = 0U; i < PHY_NUM; i++) {
		/* skip the reserved registers space */
		if (i >= 121U && i <= 255U) {
			continue;
		}
		if (i >= 377U && i <= 511U) {
			continue;
		}
		if (i >= 633U && i <= 767U) {
			continue;
		}
		if (i >= 889U && i <= 1023U) {
			continue;
		}
		if (i >= 1065U && i <= 1279U) {
			continue;
		}
		if (i >= 1321U && i <= 1535U) {
			continue;
		}
		mmio_write_32(IMX_DDRC_BASE + 0x4000 + i * 4, dram_timing_cfg->phy_full[i]);
	}

	if (dram_class == LPDDR4_TYPE) {
		/* restore only the diff. */
		mmio_write_32(IMX_DDRC_BASE + DENALI_PHY_1537, 0x0);
		for (i = 0U; i < PHY_DIFF_NUM; i++) {
			mmio_write_32(IMX_DDRC_BASE + 0x4000 + freq_specific_reg_array[i] * 4,
				      dram_timing_cfg->phy_diff[i]);
		}
	}

	/* Re-enable MULTICAST mode */
	mmio_write_32(IMX_DDRC_BASE + DENALI_PHY_1537, PHY_FREQ_MULTICAST_EN(1));
}

void dram_enter_retention(void)
{
	unsigned int i;

	/* 1. config the PCC_LPDDR4[SSADO] to 2b'11 for ACK domain 0/1's STOP */
	mmio_setbits_32(IMX_PCC5_BASE + 0x108, 0x2 << 22);

	/*
	 * 2. Make sure the DENALI_CTL_144[LPI_WAKEUP_EN[5:0]] has the bit
	 * LPI_WAKEUP_EN[3] = 1b'1. This enables the option 'self-refresh
	 * long with mem and ctlr clk gating or self-refresh  power-down
	 * long with mem and ctlr clk gating'
	 */
	mmio_setbits_32(IMX_DDRC_BASE + DENALI_CTL_144, BIT(3) << LPI_WAKEUP_EN_SHIFT);

	/*
	 * 3a. Config SIM_LPAV LPDDR_CTRL[LPDDR_AUTO_LP_MODE_DISABLE] to 1b'0(enable
	 * the logic to automatic handles low power entry/exit. This is the recommended
	 * option over handling through software.
	 * 3b. Config the SIM_LPAV LPDDR_CTRL[SOC_LP_CMD] to 6b'101001(encoding for
	 * self_refresh with both DDR controller and DRAM clock gate. THis is mandatory
	 * since LPPDR logic will be power gated).
	 */
	mmio_clrbits_32(IMX_LPAV_SIM_BASE + LPDDR_CTRL, LPDDR_AUTO_LP_MODE_DISABLE);
	mmio_clrsetbits_32(IMX_LPAV_SIM_BASE + LPDDR_CTRL,
			   0x3f << SOC_LP_CMD_SHIFT, 0x29 << SOC_LP_CMD_SHIFT);

	/* Save DDR Controller & PHY config.
	 * Set PHY_FREQ_SEL_MULTICAST_EN=0 & PHY_FREQ_SEL_INDEX=1. Read and store all
	 * the PHY registers for F2 into phy_f1_cfg, then read/store the diff between
	 * F1 & F2 into phy_f2_cfg.
	 */
	if (!dram_cfg_saved) {
		info = (struct dram_timing_info *)SAVED_DRAM_DATA_BASE;
		dram_timing_cfg = (struct dram_cfg *)(SAVED_DRAM_DATA_BASE +
					sizeof(struct dram_timing_info));

		/* get the dram type */
		dram_class = mmio_read_32(IMX_DDRC_BASE + DENALI_CTL_00);
		dram_class = (dram_class >> 8) & 0xf;

		/* save the ctl registers */
		for (i = 0U; i < CTL_NUM; i++) {
			dram_timing_cfg->ctl_cfg[i] = mmio_read_32(IMX_DDRC_BASE + i * 4);
		}
		dram_timing_cfg->ctl_cfg[0] = dram_timing_cfg->ctl_cfg[0] & 0xFFFFFFFE;

		/* save the PI registers */
		for (i = 0U; i < PI_NUM; i++) {
			dram_timing_cfg->pi_cfg[i] = mmio_read_32(IMX_DDRC_BASE + 0x2000 + i * 4);
		}
		dram_timing_cfg->pi_cfg[0] = dram_timing_cfg->pi_cfg[0] & 0xFFFFFFFE;

		/*
		 * Read and store all PHY registers. full array is a full
		 * copy for all the setpoint
		 */
		if (dram_class == LPDDR4_TYPE) {
			mmio_write_32(IMX_DDRC_BASE + DENALI_PHY_1537, 0x10000);
			for (i = 0U; i < PHY_NUM; i++) {
				/* Make sure MULTICASE is enabled */
				if (i == 1537U) {
					dram_timing_cfg->phy_full[i] = 0x100;
				} else {
					dram_timing_cfg->phy_full[i] = mmio_read_32(IMX_DDRC_BASE + 0x4000 + i * 4);
				}
			}

			/*
			 * set PHY_FREQ_SEL_MULTICAST_EN=0 & PHY_FREQ_SEL_INDEX=0.
			 * Read and store only the diff.
			 */
			mmio_write_32(IMX_DDRC_BASE + DENALI_PHY_1537, 0x0);
			/* save only the frequency based diff config to save memory */
			for (i = 0U; i < PHY_DIFF_NUM; i++) {
				dram_timing_cfg->phy_diff[i] = mmio_read_32(IMX_DDRC_BASE + 0x4000 +
									    freq_specific_reg_array[i] * 4);
			}
		} else {
			/* LPDDR3, only f1 need to save */
			for (i = 0U; i < info->phy_f1_cfg_num; i++) {
				info->phy_f1_cfg[i].val = mmio_read_32(info->phy_f1_cfg[i].reg);
			}
		}

		dram_cfg_saved = true;
	}
}

void dram_exit_retention(void)
{
	uint32_t val;

	/* 1. Config the LPAV PLL4 and DDR clock for the desired LPDDR operating frequency. */
	mmio_setbits_32(IMX_PCC5_BASE + 0x108, BIT(30));

	/* 2. Write PCC5.PCC_LPDDR4[SWRST] to 1b'1 to release LPDDR from reset. */
	mmio_setbits_32(IMX_PCC5_BASE + 0x108, BIT(28));

	/* 3. Reload the LPDDR CTL/PI/PHY register */
	ddr_init();

	if (dram_class == LPDDR4_TYPE) {
		/* 4a. FIXME Set PHY_SET_DFI_INPUT_N parameters to 4'h1. LPDDR4 only */
		mmio_write_32(IMX_DDRC_BASE + DENALI_PHY_1559, 0x01010101);

		/*
		 * 4b. CTL PWRUP_SREFRESH_EXIT=1'b0 for disabling self refresh exit
		 * from controller.
		 */
		/*
		 * 4c. PI_PWRUP_SELF_REF_EXIT=1, PI_MC_PWRUP_SELF_REF_EXIT=0 for enabling
		 * self refresh exit from PI
		 */
		/* 4c. PI_INT_LVL_EN=0 to skip Initialization trainings. */
		/*
		 * 4d. PI_WRLVL_EN_F0/1/2= PI_CALVL_EN_F0/1/2= PI_RDLVL_EN_F0/1/2=
		 * PI_RDLVL_GATE_EN_F0/1/2= PI_WDQLVL_EN_F0/1/2=0x2.
		 * Enable non initialization trainings.
		 */
		/* 4e. PI_PWRUP_SREFRESH_EXIT_CS=0xF */
		/* 4f. PI_DLL_RESET=0x1 */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_137, 0x1);
		/* PI_PWRUP_SELF_REF_EXIT = 1 */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_132, 0x01000000);
		/* PI_MC_PWRUP_SELF_REF_EXIT = 0 */
		mmio_clrbits_32(IMX_DDRC_BASE + DENALI_PI_132, BIT(16));
		/* PI_INT_LVL_EN = 0 */
		mmio_clrbits_32(IMX_DDRC_BASE + DENALI_PI_04, BIT(0));
		/* PI_WRLVL_EN_F0 = 3, PI_WRLVL_EN_F1 = 3 */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_174, 0x03030000);
		/* PI_WRLVL_EN_F2 = 3 */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_175, 0x03);
		/* PI_CALVL_EN_F0 = 3, PI_CALVL_EN_F1 = 3 */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_191, 0x03030000);
		/* PI_CALVL_EN_F2 = 3 */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_192, 0x03);
		/* PI_WDQLVL_EN_F0 = 3 */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_212, 0x300);
		/* PI_WDQLVL_EN_F1 = 3 */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_214, 0x03000000);
		/* PI_WDQLVL_EN_F2 = 3 */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_217, 0x300);
		/* PI_EDLVL_EN_F0 = 3, PI_EDLVL_GATE_EN_F0 = 3 */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_181, 0x03030000);
		/*
		 * PI_RDLVL_EN_F1 = 3, PI_RDLVL_GATE_EN_F1 = 3,
		 * PI_RDLVL_EN_F2 = 3, PI_RDLVL_GATE_EN_F2 = 3
		 */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_182, 0x03030303);
		/* PI_PWRUP_SREFRESH_EXIT_CS = 0xF */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_134, 0x000F0000);
	} else {
		/* PI_DLL_RESET=1 */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_137, 0x1);
		/* PI_PWRUP_SELF_REF_EXIT=1 */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_132, 0x01000000);
		/* PI_MC_PWRUP_SELF_REF_EXIT=0 */
		mmio_clrbits_32(IMX_DDRC_BASE + DENALI_PI_132, BIT(16));
		/* PI_INT_LVL_EN=0 */
		mmio_clrbits_32(IMX_DDRC_BASE + DENALI_PI_04, BIT(0));
		/* PI_WRLVL_EN_F0=3 */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_174, 0x00030000);
		/* PI_CALVL_EN_F0=3 */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_191, 0x00030000);
		/* PI_RDLVL_EN_F0=3,PI_RDLVL_GATE_EN_F0=3 */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_181, 0x03030000);
		/* PI_PWRUP_SREFRESH_EXIT_CS=0xF */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_134, 0x000F0000);
	}

	mmio_write_32(IMX_DDRC_BASE + DENALI_CTL_144, 0x00002D00);

	/* Force in-order AXI read data */
	mmio_write_32(IMX_DDRC_BASE + DENALI_CTL_144, 0x1);

	/*
	 * Disable special R/W group switches so that R/W group placement
	 * is always at END of R/W group.
	 */
	mmio_write_32(IMX_DDRC_BASE + DENALI_CTL_249, 0x0);

	/* Reduce time for IO pad calibration */
	mmio_write_32(IMX_DDRC_BASE + DENALI_PHY_1590, 0x01000000);

	mmio_write_32(IMX_DDRC_BASE + DENALI_CTL_25, 0x00020100);

	/* PD disable */
	mmio_write_32(IMX_DDRC_BASE + DENALI_CTL_153, 0x04040000);
	/*
	 * 5. Disable automatic LP entry and PCPCS modes LP_AUTO_ENTRY_EN
	 * to 1b'0, PCPCS_PD_EN to 1b'0
	 */

	upwr_xcp_set_ddr_retention(APD_DOMAIN, 0, NULL);
	upower_wait_resp();

	if (dram_class == LPDDR4_TYPE) {
		/* 7. Write PI START parameter to 1'b1 */
		mmio_write_32(IMX_DDRC_BASE + DENALI_PI_00, 0x00000b01);

		/* 8. Write CTL START parameter to 1'b1 */
		mmio_write_32(IMX_DDRC_BASE + DENALI_CTL_00, 0x00000b01);
	} else {
		/* 7. Write PI START parameter to 1'b1 */
		mmio_write_32(IMX_DDRC_BASE + DENALI_PI_00, 0x00000701);

		/* 8. Write CTL START parameter to 1'b1 */
		mmio_write_32(IMX_DDRC_BASE + DENALI_CTL_00, 0x00000701);
	}

	/* 9. DENALI_CTL_266:  Wait for INT_STATUS_INIT=0x2 */
	do {
		val = (mmio_read_32(IMX_DDRC_BASE + DENALI_CTL_266) >> 8) & 0xFF;
	} while (val != 0x2);

	/*
	 * 10. Run SW trainings by setting PI_CALVL_REQ,PI_WRLVL_REQ,PI_RDLVL_GATE_REQ,
	 * PI_RDLVL_REQ,PI_WDQLVL_REQ(NA for LPDDR3) in same order.
	 */
	if (dram_class == LPDDR4_TYPE) {
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_52, 0x10000); /* CALVL */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_26, 0x100); /* WRLVL */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_33, 0x10000); /* RDGATE */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_33, 0x100); /* RDQLVL */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_65, 0x10000); /* WDQLVL */

		/* 11. Wait for trainings to get complete by polling PI_INT_STATUS */
		while ((mmio_read_32(IMX_DDRC_BASE + DENALI_PI_77) & 0x07E00000) != 0x07E00000) {
			;
		}
	} else {
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_52, 0x10000); /* CALVL */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_26, 0x100); /* WRLVL */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_33, 0x10000); /* RDGATE */
		mmio_setbits_32(IMX_DDRC_BASE + DENALI_PI_33, 0x100); /* RDQLVL */
		while ((mmio_read_32(IMX_DDRC_BASE + DENALI_PI_77) & 0x05E00000) != 0x05E00000) {
			;
		}
	}
}

#define LPDDR_DONE       (0x1<<4)
#define SOC_FREQ_CHG_ACK (0x1<<6)
#define SOC_FREQ_CHG_REQ (0x1<<7)
#define LPI_WAKEUP_EN    (0x4<<8)
#define SOC_FREQ_REQ     (0x1<<11)

#define LPDDR_EN_CLKGATE (0x1<<17)

static void set_cgc2_ddrclk(uint8_t src, uint8_t div)
{

	/* Wait until the reg is unlocked for writing */
	while (mmio_read_32(IMX_CGC2_BASE + 0x40) & BIT(31))
		;

	mmio_write_32(IMX_CGC2_BASE + 0x40, (src << 28) | (div << 21));
	/* Wait for the clock switching done */
	while (!(mmio_read_32(IMX_CGC2_BASE + 0x40) & BIT(27)))
		;
}
static void set_ddr_clk(uint32_t ddr_freq)
{
	/* Disable DDR clock */
	mmio_clrbits_32(IMX_PCC5_BASE + 0x108, BIT(30));
	switch (ddr_freq) {
	/* boot frequency ? */
	case 48:
		set_cgc2_ddrclk(2, 0);
		break;
	/* default bypass frequency for fsp 1 */
	case 192:
		set_cgc2_ddrclk(0, 1);
		break;
	case 384:
		set_cgc2_ddrclk(0, 0);
		break;
	case 264:
		set_cgc2_ddrclk(4, 3);
		break;
	case 528:
		set_cgc2_ddrclk(4, 1);
		break;
	default:
		break;
	}
	/* Enable DDR clock */
	mmio_setbits_32(IMX_PCC5_BASE + 0x108, BIT(30));

	/* Wait until the reg is unlocked for writing */
	while (mmio_read_32(IMX_CGC2_BASE + 0x40) & BIT(31)) {
		;
	}
}

#define AVD_SIM_LPDDR_CTRL	(IMX_LPAV_SIM_BASE + 0x14)
#define AVD_SIM_LPDDR_CTRL2	(IMX_LPAV_SIM_BASE + 0x18)
#define MAX_FSP_NUM	U(3)
#define DDR_DFS_GET_FSP_COUNT	0x10
#define DDR_BYPASS_DRATE	U(400)

extern int upower_pmic_i2c_write(uint32_t reg_addr, uint32_t reg_val);

/* Normally, we only switch frequency between 1(bypass) and 2(highest) */
int lpddr4_dfs(uint32_t freq_index)
{
	uint32_t lpddr_ctrl, lpddr_ctrl2;
	uint32_t ddr_ctl_144;

	/*
	 * Valid index: 0 to 2
	 * index 0: boot frequency
	 * index 1: bypass frequency
	 * index 2: highest frequency
	 */
	if (freq_index > 2U) {
		return -1;
	}

	/*
	 * increase the voltage to 1.1V firstly before increase frequency
	 * and APD enter OD mode
	 */
	if (freq_index == 2U && sys_dvfs) {
		upower_pmic_i2c_write(0x22, 0x28);
	}

	/* Enable LPI_WAKEUP_EN */
	ddr_ctl_144 = mmio_read_32(IMX_DDRC_BASE + DENALI_CTL_144);
	mmio_setbits_32(IMX_DDRC_BASE + DENALI_CTL_144, LPI_WAKEUP_EN);

	/* put DRAM into long self-refresh & clock gating */
	lpddr_ctrl = mmio_read_32(AVD_SIM_LPDDR_CTRL);
	lpddr_ctrl = (lpddr_ctrl & ~((0x3f << 15) | (0x3 << 9))) | (0x28 << 15) | (freq_index << 9);
	mmio_write_32(AVD_SIM_LPDDR_CTRL, lpddr_ctrl);

	/* Gating the clock */
	lpddr_ctrl2 = mmio_read_32(AVD_SIM_LPDDR_CTRL2);
	mmio_setbits_32(AVD_SIM_LPDDR_CTRL2, LPDDR_EN_CLKGATE);

	/* Request frequency change */
	mmio_setbits_32(AVD_SIM_LPDDR_CTRL, SOC_FREQ_REQ);

	do {
		lpddr_ctrl = mmio_read_32(AVD_SIM_LPDDR_CTRL);
		if (lpddr_ctrl & SOC_FREQ_CHG_REQ) {
			/* Bypass mode */
			if (info->fsp_table[freq_index] < DDR_BYPASS_DRATE) {
				/* Change to PLL bypass mode */
				mmio_write_32(IMX_LPAV_SIM_BASE, 0x1);
				/* change the ddr clock source & frequency */
				set_ddr_clk(info->fsp_table[freq_index]);
			} else {
				/* Change to PLL unbypass mode */
				mmio_write_32(IMX_LPAV_SIM_BASE, 0x0);
				/* change the ddr clock source & frequency */
				set_ddr_clk(info->fsp_table[freq_index] >> 1);
			}

			mmio_clrsetbits_32(AVD_SIM_LPDDR_CTRL, SOC_FREQ_CHG_REQ, SOC_FREQ_CHG_ACK);
			continue;
		}
	} while ((lpddr_ctrl & LPDDR_DONE) != 0); /* several try? */

	/* restore the original setting */
	mmio_write_32(IMX_DDRC_BASE + DENALI_CTL_144, ddr_ctl_144);
	mmio_write_32(AVD_SIM_LPDDR_CTRL2, lpddr_ctrl2);

	/* Check the DFS result */
	lpddr_ctrl = mmio_read_32(AVD_SIM_LPDDR_CTRL) & 0xF;
	if (lpddr_ctrl != 0U) {
		/* Must be something wrong, return failure */
		return -1;
	}

	/* decrease the BUCK3 voltage after frequency changed to lower
	 * and APD in ND_MODE
	 */
	if (freq_index == 1U && sys_dvfs) {
		upower_pmic_i2c_write(0x22, 0x20);
	}

	/* DFS done successfully */
	return 0;
}

/* for the non-primary core, waiting for DFS done */
static uint64_t waiting_dvfs(uint32_t id, uint32_t flags,
		void *handle, void *cookie)
{
	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);
	core_count++;
	dsb();
	spin_unlock(&dfs_lock);

	while (in_progress) {
		wfe();
	}

	return 0;
}

int dram_dvfs_handler(uint32_t smc_fid, void *handle,
		u_register_t x1, u_register_t x2, u_register_t x3)
{
	unsigned int fsp_index = x1;
	uint32_t online_cpus = x2 - 1;
	uint64_t mpidr = read_mpidr_el1();
	unsigned int cpu_id = MPIDR_AFFLVL0_VAL(mpidr);

	/* Get the number of FSPs */
	if (x1 == DDR_DFS_GET_FSP_COUNT) {
		SMC_RET2(handle, num_fsp, info->fsp_table[1]);
	}

	/* start lpddr frequency scaling */
	in_progress = true;
	sys_dvfs = x3 ? true : false;
	dsb();

	/* notify other core wait for scaling done */
	for (unsigned int i = 0; i < PLATFORM_CORE_COUNT; i++)
		/* Skip raise SGI for current CPU */
		if (i != cpu_id) {
			plat_ic_raise_el3_sgi(0x8, i);
		}

	/* Make sure all the cpu in WFE */
	while (online_cpus != core_count) {
		;
	}

	/* Flush the L1/L2 cache */
	dcsw_op_all(DCCSW);

	lpddr4_dfs(fsp_index);

	in_progress = false;
	core_count = 0;
	dsb();
	sev();
	isb();

	SMC_RET1(handle, 0);
}

void dram_init(void)
{
	uint32_t flags = 0;
	uint32_t rc;
	unsigned int i;

	/* 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) {
		panic();
	}

	info = (struct dram_timing_info *)SAVED_DRAM_DATA_BASE;

	/* Get the num of the supported Fsp */
	for (i = 0; i < MAX_FSP_NUM; i++) {
		if (!info->fsp_table[i]) {
			break;
		}
	}

	num_fsp = (i > MAX_FSP_NUM) ? MAX_FSP_NUM : i;
}