14 files changed, 3111 insertions, 0 deletions
diff --git a/plat/intel/soc/common/drivers/ccu/ncore_ccu.c b/plat/intel/soc/common/drivers/ccu/ncore_ccu.c
new file mode 100644
index 0000000..684a625
--- /dev/null
+++ b/plat/intel/soc/common/drivers/ccu/ncore_ccu.c
@@ -0,0 +1,146 @@
+/*
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <assert.h>
+#include <common/debug.h>
+#include <errno.h>
+#include <lib/mmio.h>
+#include <platform_def.h>
+
+#include "ncore_ccu.h"
+#include "socfpga_plat_def.h"
+#include "socfpga_system_manager.h"
+
+uint32_t poll_active_bit(uint32_t dir);
+
+#define SMMU_DMI			1
+
+
+static coh_ss_id_t subsystem_id;
+void get_subsystem_id(void)
+{
+	uint32_t snoop_filter, directory, coh_agent;
+	snoop_filter = CSIDR_NUM_SF(mmio_read_32(NCORE_CCU_CSR(NCORE_CSIDR)));
+	directory = CSUIDR_NUM_DIR(mmio_read_32(NCORE_CCU_CSR(NCORE_CSUIDR)));
+	coh_agent = CSUIDR_NUM_CAI(mmio_read_32(NCORE_CCU_CSR(NCORE_CSUIDR)));
+	subsystem_id.num_snoop_filter = snoop_filter + 1;
+	subsystem_id.num_directory = directory;
+	subsystem_id.num_coh_agent = coh_agent;
+}
+uint32_t directory_init(void)
+{
+	uint32_t dir_sf_mtn, dir_sf_en;
+	uint32_t dir, sf, ret;
+	for (dir = 0; dir < subsystem_id.num_directory; dir++) {
+		for (sf = 0; sf < subsystem_id.num_snoop_filter; sf++) {
+			dir_sf_mtn = DIRECTORY_UNIT(dir, NCORE_DIRUSFMCR);
+			dir_sf_en = DIRECTORY_UNIT(dir, NCORE_DIRUSFER);
+			/* Initialize All Entries */
+			mmio_write_32(dir_sf_mtn, SNOOP_FILTER_ID(sf));
+			/* Poll Active Bit */
+			ret = poll_active_bit(dir);
+			if (ret != 0) {
+				ERROR("Timeout during active bit polling");
+				return -ETIMEDOUT;
+			}
+			/* Disable snoop filter, a bit per snoop filter */
+			mmio_clrbits_32(dir_sf_en, BIT(sf));
+		}
+	}
+	return 0;
+}
+uint32_t coherent_agent_intfc_init(void)
+{
+	uint32_t dir, ca, ca_id, ca_type, ca_snoop_en;
+	for (dir = 0; dir < subsystem_id.num_directory; dir++) {
+		for (ca = 0; ca < subsystem_id.num_coh_agent; ca++) {
+			ca_snoop_en = DIRECTORY_UNIT(ca, NCORE_DIRUCASER0);
+			ca_id = mmio_read_32(COH_AGENT_UNIT(ca, NCORE_CAIUIDR));
+			/* Coh Agent Snoop Enable */
+			if (CACHING_AGENT_BIT(ca_id))
+				mmio_setbits_32(ca_snoop_en, BIT(ca));
+			/* Coh Agent Snoop DVM Enable */
+			ca_type = CACHING_AGENT_TYPE(ca_id);
+			if (ca_type == ACE_W_DVM || ca_type == ACE_L_W_DVM)
+				mmio_setbits_32(NCORE_CCU_CSR(NCORE_CSADSER0),
+				BIT(ca));
+		}
+	}
+	return 0;
+}
+uint32_t poll_active_bit(uint32_t dir)
+{
+	uint32_t timeout = 80000;
+	uint32_t poll_dir =  DIRECTORY_UNIT(dir, NCORE_DIRUSFMAR);
+	while (timeout > 0) {
+		if (mmio_read_32(poll_dir) == 0)
+			return 0;
+		timeout--;
+	}
+	return -1;
+}
+void bypass_ocram_firewall(void)
+{
+	mmio_clrbits_32(COH_CPU0_BYPASS_REG(NCORE_FW_OCRAM_BLK_CGF1),
+			OCRAM_PRIVILEGED_MASK | OCRAM_SECURE_MASK);
+	mmio_clrbits_32(COH_CPU0_BYPASS_REG(NCORE_FW_OCRAM_BLK_CGF2),
+			OCRAM_PRIVILEGED_MASK | OCRAM_SECURE_MASK);
+	mmio_clrbits_32(COH_CPU0_BYPASS_REG(NCORE_FW_OCRAM_BLK_CGF3),
+			OCRAM_PRIVILEGED_MASK | OCRAM_SECURE_MASK);
+	mmio_clrbits_32(COH_CPU0_BYPASS_REG(NCORE_FW_OCRAM_BLK_CGF4),
+			OCRAM_PRIVILEGED_MASK | OCRAM_SECURE_MASK);
+}
+void ncore_enable_ocram_firewall(void)
+{
+	mmio_setbits_32(COH_CPU0_BYPASS_REG(NCORE_FW_OCRAM_BLK_CGF1),
+			OCRAM_PRIVILEGED_MASK | OCRAM_SECURE_MASK);
+	mmio_setbits_32(COH_CPU0_BYPASS_REG(NCORE_FW_OCRAM_BLK_CGF2),
+			OCRAM_PRIVILEGED_MASK | OCRAM_SECURE_MASK);
+	mmio_setbits_32(COH_CPU0_BYPASS_REG(NCORE_FW_OCRAM_BLK_CGF3),
+			OCRAM_PRIVILEGED_MASK | OCRAM_SECURE_MASK);
+	mmio_setbits_32(COH_CPU0_BYPASS_REG(NCORE_FW_OCRAM_BLK_CGF4),
+			OCRAM_PRIVILEGED_MASK | OCRAM_SECURE_MASK);
+}
+uint32_t init_ncore_ccu(void)
+{
+	uint32_t status;
+	get_subsystem_id();
+	status = directory_init();
+	status = coherent_agent_intfc_init();
+	bypass_ocram_firewall();
+	return status;
+}
+
+void setup_smmu_stream_id(void)
+{
+	/* Configure Stream ID for Agilex5 */
+	mmio_write_32(SOCFPGA_SYSMGR(DMA_TBU_STREAM_ID_AX_REG_0_DMA0), DMA0);
+	mmio_write_32(SOCFPGA_SYSMGR(DMA_TBU_STREAM_ID_AX_REG_0_DMA1), DMA1);
+	mmio_write_32(SOCFPGA_SYSMGR(SDM_TBU_STREAM_ID_AX_REG_1_SDM), SDM);
+	/* Reg map showing USB2 but Linux USB0? */
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_ID_AX_REG_2_USB2), USB0);
+	/* Reg map showing USB3 but Linux USB1? */
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_ID_AX_REG_2_USB3), USB1);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_ID_AX_REG_2_SDMMC), SDMMC);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_ID_AX_REG_2_NAND), NAND);
+	/* To confirm ETR - core sight debug*/
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_ID_AX_REG_2_ETR), CORE_SIGHT_DEBUG);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_ID_AX_REG_2_TSN0), TSN0);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_ID_AX_REG_2_TSN1), TSN1);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_ID_AX_REG_2_TSN2), TSN2);
+
+	/* Enabled Stream ctrl register for Agilex5 */
+	mmio_write_32(SOCFPGA_SYSMGR(DMA_TBU_STREAM_CTRL_REG_0_DMA0), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(DMA_TBU_STREAM_CTRL_REG_0_DMA1), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(SDM_TBU_STREAM_CTRL_REG_1_SDM), ENABLE_STREAMID_SECURE_TX);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_CTRL_REG_2_USB2), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_CTRL_REG_2_USB3), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_CTRL_REG_2_SDMMC), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_CTRL_REG_2_NAND), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_CTRL_REG_2_ETR), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(TSN_TBU_STREAM_CTRL_REG_3_TSN0), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(TSN_TBU_STREAM_CTRL_REG_3_TSN1), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(TSN_TBU_STREAM_CTRL_REG_3_TSN2), ENABLE_STREAMID);
+}
diff --git a/plat/intel/soc/common/drivers/ccu/ncore_ccu.h b/plat/intel/soc/common/drivers/ccu/ncore_ccu.h
new file mode 100644
index 0000000..6cdbeb8
--- /dev/null
+++ b/plat/intel/soc/common/drivers/ccu/ncore_ccu.h
@@ -0,0 +1,431 @@
+/*
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef NCORE_CCU_H
+#define NCORE_CCU_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#ifndef CCU_ACTIVATE_COH_FPGA
+#define CCU_ACTIVATE_COH_FPGA 0
+#endif
+// Address map for ccu init
+#define addr_CAIUIDR1				(0x1C000000)
+#define addr_GRBUNRRUCR				(0x1c0ffff8)
+#define base_addr_NRS_CAIU0			(0x1c000000)
+#define base_addr_NRS_NCAIU0			(0x1c001000)
+#define base_addr_NRS_NCAIU1			(0x1c002000)
+#define base_addr_NRS_NCAIU2			(0x1c003000)
+#define base_addr_NRS_NCAIU3			(0x1c004000)
+#define base_addr_NRS_DCE0			(0x1c005000)
+#define base_addr_NRS_DCE1			(0x1c006000)
+//#define base_addr_NRS_DMI0			(0x1c007000)
+//#define base_addr_NRS_DMI1			(0x1c008000)
+//DMI
+#define ALT_CCU_CCU_DMI0_DMIUSMCTCR_ADDR	0x1C007300
+#define ALT_CCU_CCU_DMI1_DMIUSMCTCR_ADDR	0x1C008300
+//DSU
+#define ALT_CCU_DSU_CAIUAMIGR_ADDR		0x1C0003C0
+#define ALT_CCU_DSU_CAIUMIFSR_ADDR		0x1C0003C4
+#define ALT_CCU_DSU_CAIUGPRBLR1_ADDR		0x1C000414
+#define ALT_CCU_DSU_CAIUGPRBHR1_ADDR		0x1C000418
+#define ALT_CCU_DSU_CAIUGPRAR1_ADDR		0x1C000410
+#define ALT_CCU_DSU_CAIUGPRBLR2_ADDR		0x1C000424
+#define ALT_CCU_DSU_CAIUGPRBHR2_ADDR		0x1C000428
+#define ALT_CCU_DSU_CAIUGPRAR2_ADDR		0x1C000420
+#define ALT_CCU_DSU_CAIUGPRBLR4_ADDR		0x1C000444
+#define ALT_CCU_DSU_CAIUGPRBHR4_ADDR		0x1C000448
+#define ALT_CCU_DSU_CAIUGPRAR4_ADDR		0x1C000440
+#define ALT_CCU_DSU_CAIUGPRBLR5_ADDR		0x1C000454
+#define ALT_CCU_DSU_CAIUGPRBHR5_ADDR		0x1C000458
+#define ALT_CCU_DSU_CAIUGPRAR5_ADDR		0x1C000450
+#define ALT_CCU_DSU_CAIUGPRBLR6_ADDR		0x1C000464
+#define ALT_CCU_DSU_CAIUGPRBHR6_ADDR		0x1C000468
+#define ALT_CCU_DSU_CAIUGPRAR6_ADDR		0x1C000460
+#define ALT_CCU_DSU_CAIUGPRBLR7_ADDR		0x1C000474
+#define ALT_CCU_DSU_CAIUGPRBHR7_ADDR		0x1C000478
+#define ALT_CCU_DSU_CAIUGPRAR7_ADDR		0x1C000470
+#define ALT_CCU_DSU_CAIUGPRBLR8_ADDR		0x1C000484
+#define ALT_CCU_DSU_CAIUGPRBHR8_ADDR		0x1C000488
+#define ALT_CCU_DSU_CAIUGPRAR8_ADDR		0x1C000480
+#define ALT_CCU_DSU_CAIUGPRBLR9_ADDR		0x1C000494
+#define ALT_CCU_DSU_CAIUGPRBHR9_ADDR		0x1C000498
+#define ALT_CCU_DSU_CAIUGPRAR9_ADDR		0x1C000490
+#define ALT_CCU_DSU_CAIUGPRBLR10_ADDR		0x1C0004A4
+#define ALT_CCU_DSU_CAIUGPRBHR10_ADDR		0x1C0004A8
+#define ALT_CCU_DSU_CAIUGPRAR10_ADDR		0x1C0004A0
+//GIC
+#define ALT_CCU_GIC_M_XAIUAMIGR_ADDR		0x1C0023C0
+#define ALT_CCU_GIC_M_XAIUMIFSR_ADDR		0x1C0023C4
+#define ALT_CCU_GIC_M_XAIUGPRBLR1_ADDR		0x1C002414
+#define ALT_CCU_GIC_M_XAIUGPRBHR1_ADDR		0x1C002418
+#define ALT_CCU_GIC_M_XAIUGPRAR1_ADDR		0x1C002410
+#define ALT_CCU_GIC_M_XAIUGPRBLR6_ADDR		0x1C002464
+#define ALT_CCU_GIC_M_XAIUGPRBHR6_ADDR		0x1C002468
+#define ALT_CCU_GIC_M_XAIUGPRAR6_ADDR		0x1C002460
+#define ALT_CCU_GIC_M_XAIUGPRBLR8_ADDR		0x1C002484
+#define ALT_CCU_GIC_M_XAIUGPRBHR8_ADDR		0x1C002488
+#define ALT_CCU_GIC_M_XAIUGPRAR8_ADDR		0x1C002480
+#define ALT_CCU_GIC_M_XAIUGPRBLR10_ADDR		0x1C0024A4
+#define ALT_CCU_GIC_M_XAIUGPRBHR10_ADDR		0x1C0024A8
+#define ALT_CCU_GIC_M_XAIUGPRAR10_ADDR		0x1C0024A0
+//FPGA2SOC
+#define ALT_CCU_FPGA2SOC_XAIUAMIGR_ADDR		0x1C0013C0
+#define ALT_CCU_FPGA2SOC_XAIUMIFSR_ADDR		0x1C0013C4
+#define ALT_CCU_FPGA2SOC_XAIUGPRBLR1_ADDR	0x1C001414
+#define ALT_CCU_FPGA2SOC_XAIUGPRBHR1_ADDR	0x1C001418
+#define ALT_CCU_FPGA2SOC_XAIUGPRAR1_ADDR	0x1C001410
+#define ALT_CCU_FPGA2SOC_XAIUGPRBLR6_ADDR	0x1C001464
+#define ALT_CCU_FPGA2SOC_XAIUGPRBHR6_ADDR	0x1C001468
+#define ALT_CCU_FPGA2SOC_XAIUGPRAR6_ADDR	0x1C001460
+#define ALT_CCU_FPGA2SOC_XAIUGPRBLR8_ADDR	0x1C001484
+#define ALT_CCU_FPGA2SOC_XAIUGPRBHR8_ADDR	0x1C001488
+#define ALT_CCU_FPGA2SOC_XAIUGPRAR8_ADDR	0x1C001480
+#define ALT_CCU_FPGA2SOC_XAIUGPRBLR10_ADDR	0x1C0014A4
+#define ALT_CCU_FPGA2SOC_XAIUGPRBHR10_ADDR	0x1C0014A8
+#define ALT_CCU_FPGA2SOC_XAIUGPRAR10_ADDR	0x1C0014A0
+//TCU
+#define ALT_CCU_TCU_BASE 0x1C003000
+#define ALT_CCU_TCU_XAIUAMIGR_ADDR		ALT_CCU_TCU_BASE + 0x03C0
+#define ALT_CCU_TCU_XAIUMIFSR_ADDR		ALT_CCU_TCU_BASE + 0x03C4
+#define ALT_CCU_TCU_XAIUGPRBLR0_ADDR		ALT_CCU_TCU_BASE + 0x0404
+#define ALT_CCU_TCU_XAIUGPRBHR0_ADDR		ALT_CCU_TCU_BASE + 0x0408
+#define ALT_CCU_TCU_XAIUGPRAR0_ADDR		ALT_CCU_TCU_BASE + 0x0400
+#define ALT_CCU_TCU_XAIUGPRBLR1_ADDR		ALT_CCU_TCU_BASE + 0x0414
+#define ALT_CCU_TCU_XAIUGPRBHR1_ADDR		ALT_CCU_TCU_BASE + 0x0418
+#define ALT_CCU_TCU_XAIUGPRAR1_ADDR		ALT_CCU_TCU_BASE + 0x0410
+#define ALT_CCU_TCU_XAIUGPRBLR2_ADDR		ALT_CCU_TCU_BASE + 0x0424
+#define ALT_CCU_TCU_XAIUGPRBHR2_ADDR		ALT_CCU_TCU_BASE + 0x0428
+#define ALT_CCU_TCU_XAIUGPRAR2_ADDR		ALT_CCU_TCU_BASE + 0x0420
+#define ALT_CCU_TCU_XAIUGPRBLR6_ADDR		0x1C003464
+#define ALT_CCU_TCU_XAIUGPRBHR6_ADDR		0x1C003468
+#define ALT_CCU_TCU_XAIUGPRAR6_ADDR		0x1C003460
+#define ALT_CCU_TCU_XAIUGPRBLR8_ADDR		0x1C003484
+#define ALT_CCU_TCU_XAIUGPRBHR8_ADDR		0x1C003488
+#define ALT_CCU_TCU_XAIUGPRAR8_ADDR		0x1C003480
+#define ALT_CCU_TCU_XAIUGPRBLR10_ADDR		0x1C0034A4
+#define ALT_CCU_TCU_XAIUGPRBHR10_ADDR		0x1C0034A8
+#define ALT_CCU_TCU_XAIUGPRAR10_ADDR		0x1C0034A0
+//IOM
+#define ALT_CCU_CCU_IOM_XAIUAMIGR_ADDR		0x1C0043C0
+#define ALT_CCU_CCU_IOM_XAIUMIFSR_ADDR		0x1C0013C4
+#define ALT_CCU_IOM_XAIUGPRBLR1_ADDR		0x1C001414
+#define ALT_CCU_IOM_XAIUGPRBHR1_ADDR		0x1C001418
+#define ALT_CCU_IOM_XAIUGPRAR1_ADDR		0x1C001410
+#define ALT_CCU_CCU_IOM_XAIUGPRBLR6_ADDR	0x1C001464
+#define ALT_CCU_CCU_IOM_XAIUGPRBHR6_ADDR	0x1C001468
+#define ALT_CCU_CCU_IOM_XAIUGPRAR6_ADDR		0x1C001460
+#define ALT_CCU_CCU_IOM_XAIUGPRBLR8_ADDR	0x1C001484
+#define ALT_CCU_CCU_IOM_XAIUGPRBHR8_ADDR	0x1C001488
+#define ALT_CCU_CCU_IOM_XAIUGPRAR8_ADDR		0x1C001480
+#define ALT_CCU_CCU_IOM_XAIUGPRBLR10_ADDR	0x1C0014A4
+#define ALT_CCU_CCU_IOM_XAIUGPRBHR10_ADDR	0x1C0014A8
+#define ALT_CCU_CCU_IOM_XAIUGPRAR10_ADDR	0x1C0014A0
+//DCE
+#define ALT_CCU_DCE0_DCEUAMIGR_ADDR		0x1C0053C0
+#define ALT_CCU_DCE0_DCEUMIFSR_ADDR		0x1C0053C4
+#define ALT_CCU_DCE0_DCEUGPRBLR6_ADDR		0x1C005464
+#define ALT_CCU_DCE0_DCEUGPRBHR6_ADDR		0x1C005468
+#define ALT_CCU_DCE0_DCEUGPRAR6_ADDR		0x1C005460
+#define ALT_CCU_DCE0_DCEUGPRBLR8_ADDR		0x1C005484
+#define ALT_CCU_DCE0_DCEUGPRBHR8_ADDR		0x1C005488
+#define ALT_CCU_DCE0_DCEUGPRAR8_ADDR		0x1C005480
+#define ALT_CCU_DCE0_DCEUGPRBLR10_ADDR		0x1C0054A4
+#define ALT_CCU_DCE0_DCEUGPRBHR10_ADDR		0x1C0054A8
+#define ALT_CCU_DCE0_DCEUGPRAR10_ADDR		0x1C0054A0
+#define ALT_CCU_DCE1_DCEUAMIGR_ADDR		0x1C0063C0
+#define ALT_CCU_DCE1_DCEUMIFSR_ADDR		0x1C0063C4
+#define ALT_CCU_DCE1_DCEUGPRBLR6_ADDR		0x1C006464
+#define ALT_CCU_DCE1_DCEUGPRBHR6_ADDR		0x1C006468
+#define ALT_CCU_DCE1_DCEUGPRAR6_ADDR		0x1C006460
+#define ALT_CCU_DCE1_DCEUGPRBLR8_ADDR		0x1C006484
+#define ALT_CCU_DCE1_DCEUGPRBHR8_ADDR		0x1C006488
+#define ALT_CCU_DCE1_DCEUGPRAR8_ADDR		0x1C006480
+#define ALT_CCU_DCE1_DCEUGPRBLR10_ADDR		0x1C0064A4
+#define ALT_CCU_DCE1_DCEUGPRBHR10_ADDR		0x1C0064A8
+#define ALT_CCU_DCE1_DCEUGPRAR10_ADDR		0x1C0064A0
+#define offset_NRS_GPRAR0			(0x400)
+#define offset_NRS_GPRBLR0			(0x404)
+#define offset_NRS_GPRBHR0			(0x408)
+#define offset_NRS_GPRAR1			(0x410)
+#define offset_NRS_GPRBLR1			(0x414)
+#define offset_NRS_GPRBHR1			(0x418)
+#define offset_NRS_GPRAR2			(0x420)
+#define offset_NRS_GPRBLR2			(0x424)
+#define offset_NRS_GPRBHR2			(0x428)
+#define offset_NRS_GPRAR3			(0x430)
+#define offset_NRS_GPRBLR3			(0x434)
+#define offset_NRS_GPRBHR3			(0x438)
+#define offset_NRS_GPRAR4			(0x440)
+#define offset_NRS_GPRBLR4			(0x444)
+#define offset_NRS_GPRBHR4			(0x448)
+#define offset_NRS_GPRAR5			(0x450)
+#define offset_NRS_GPRBLR5			(0x454)
+#define offset_NRS_GPRBHR5			(0x458)
+#define offset_NRS_GPRAR6			(0x460)
+#define offset_NRS_GPRBLR6			(0x464)
+#define offset_NRS_GPRBHR6			(0x468)
+#define offset_NRS_GPRAR7			(0x470)
+#define offset_NRS_GPRBLR7			(0x474)
+#define offset_NRS_GPRBHR7			(0x478)
+#define offset_NRS_GPRAR8			(0x480)
+#define offset_NRS_GPRBLR8			(0x484)
+#define offset_NRS_GPRBHR8			(0x488)
+#define offset_NRS_GPRAR9			(0x490)
+#define offset_NRS_GPRBLR9			(0x494)
+#define offset_NRS_GPRBHR9			(0x498)
+#define offset_NRS_GPRAR10			(0x4a0)
+#define offset_NRS_GPRBLR10			(0x4a4)
+#define offset_NRS_GPRBHR10			(0x4a8)
+#define offset_NRS_AMIGR			(0x3c0)
+#define offset_NRS_MIFSR			(0x3c4)
+#define offset_NRS_DMIUSMCTCR			(0x300)
+#define base_addr_DII0_PSSPERIPHS		(0x10000)
+#define base_addr_DII0_LWHPS2FPGA		(0x20000)
+#define base_addr_DII0_HPS2FPGA_1G		(0x40000)
+#define base_addr_DII0_HPS2FPGA_15G		(0x400000)
+#define base_addr_DII0_HPS2FPGA_240G		(0x4000000)
+#define base_addr_DII1_MPFEREGS			(0x18000)
+#define base_addr_DII2_GICREGS			(0x1D000)
+#define base_addr_DII3_OCRAM			(0x0)
+#define base_addr_BHR				(0x0)
+#define base_addr_DMI_SDRAM_2G			(0x80000)
+#define base_addr_DMI_SDRAM_30G			(0x800000)
+#define base_addr_DMI_SDRAM_480G		(0x8000000)
+// ((0x0<<9) | (0xf<<20) | (0x1<<30) | (0x1<<31))
+#define wr_DII0_PSSPERIPHS			0xC0F00000
+// ((0x0<<9) | (0x11<<20) | (0x1<<30) | (0x1<<31))
+#define wr_DII0_LWHPS2FPGA			0xC1100000
+// ((0x0<<9) | (0x12<<20) | (0x1<<30) | (0x1<<31))
+#define wr_DII0_HPS2FPGA_1G			0xC1200000
+// ((0x0<<9) | (0x16<<20) | (0x1<<30) | (0x1<<31))
+#define wr_DII0_HPS2FPGA_15G			0xC1600000
+// ((0x0<<9) | (0x1a<<20) | (0x1<<30) | (0x1<<31))
+#define wr_DII0_HPS2FPGA_240G			0xC1A00000
+// ((0x1<<9) | (0xe<<20) | (0x1<<30) | (0x1<<31))
+#define wr_DII1_MPFEREGS			0xC0E00200
+// ((0x2<<9) | (0x8<<20) | (0x1<<30) | (0x1<<31))
+#define wr_DII2_GICREGS				0xC0800400
+// ((0x3<<9) | (0x9<<20) | (0x1<<30) | (0x1<<31))
+#define wr_DII3_OCRAM				0xC0900600
+// ((0x0<<9) | (0x12<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_1G_ORDERED			0x81200000
+// ((0x1<<1) | (0x1<<2) | (0x0<<9) | (0x12<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_1G				0x81200006
+// ((0x0<<9) | (0x13<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_2G_ORDERED			0x81300000
+// ((0x1<<1) | (0x1<<2) | (0x0<<9) | (0x13<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_2G				0x81300006
+// ((0x0<<9) | (0x16<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_15G_ORDERED		0x81600000
+// ((0x1<<1) | (0x1<<2) | (0x0<<9) | (0x16<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_15G			0x81600006
+// ((0x0<<9) | (0x17<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_30G_ORDERED		0x81700000
+// ((0x1<<1) | (0x1<<2) | (0x0<<9) | (0x17<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_30G			0x81700006
+// ((0x0<<9) | (0x1a<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_240G_ORDERED		0x81a00000
+// ((0x1<<1) | (0x1<<2) | (0x0<<9) | (0x1a<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_240G			0x81a00006
+// ((0x0<<9) | (0x1b<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_480G_ORDERED		0x81b00000
+// ((0x1<<1) | (0x1<<2) | (0x0<<9) | (0x1b<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_480G			0x81b00006
+
+typedef enum CCU_REGION_SECURITY_e {
+    //
+    // Allow secure accesses only.
+    //
+	CCU_REGION_SECURITY_SECURE_ONLY,
+    //
+    // Allow non-secure accesses only.
+    //
+	CCU_REGION_SECURITY_NON_SECURE_ONLY,
+    //
+    // Allow accesses of any security state.
+    //
+	CCU_REGION_SECURITY_DONT_CARE
+} CCU_REGION_SECURITY_t;
+typedef enum CCU_REGION_PRIVILEGE_e {
+    //
+    // Allow privileged accesses only.
+    //
+	CCU_REGION_PRIVILEGE_PRIVILEGED_ONLY,
+    //
+    // Allow unprivileged accesses only.
+    //
+	CCU_REGION_PRIVILEGE_NON_PRIVILEGED_ONLY,
+    //
+    // Allow accesses of any privilege.
+    //
+	CCU_REGION_PRIVILEGE_DONT_CARE
+} CCU_REGION_PRIVILEGE_t;
+//
+// Initializes the CCU by enabling all regions except RAM 1 - 5.
+// This is needed because of an RTL change around 2016.02.24.
+//
+// Runtime measurement:
+//  - arm     : 14,830,000 ps (2016.05.31; sanity/printf_aarch32)
+//  - aarch64 : 14,837,500 ps (2016.05.31; sanity/printf)
+//
+// Runtime history:
+//  - arm     : 20,916,668 ps (2016.05.30; sanity/printf_aarch32)
+//  - aarch64 : 20,924,168 ps (2016.05.30; sanity/printf)
+//
+int ccu_hps_init(void);
+
+typedef enum ccu_hps_ram_region_e {
+	ccu_hps_ram_region_ramspace0 = 0,
+	ccu_hps_ram_region_ramspace1 = 1,
+	ccu_hps_ram_region_ramspace2 = 2,
+	ccu_hps_ram_region_ramspace3 = 3,
+	ccu_hps_ram_region_ramspace4 = 4,
+	ccu_hps_ram_region_ramspace5 = 5,
+} ccu_hps_ram_region_t;
+
+// Disables a RAM (OCRAM) region with the given ID.
+int ccu_hps_ram_region_disable(int id);
+
+// Enables a RAM (OCRAM) region with the given ID.
+int ccu_hps_ram_region_enable(int id);
+
+// Attempts to remap a RAM (OCRAM) region with the given ID to span the given
+// start and end address. It also assigns the security and privilege policy.
+// Regions must be a power-of-two size with a minimum size of 64B.
+int ccu_hps_ram_region_remap(int id, uintptr_t start, uintptr_t end,
+CCU_REGION_SECURITY_t security, CCU_REGION_PRIVILEGE_t privilege);
+
+// Verifies that all enabled RAM (OCRAM) regions does not overlap.
+int ccu_hps_ram_validate(void);
+
+typedef enum ccu_hps_mem_region_e {
+	ccu_hps_mem_region_ddrspace0  = 0,
+	ccu_hps_mem_region_memspace0  = 1,
+	ccu_hps_mem_region_memspace1a = 2,
+	ccu_hps_mem_region_memspace1b = 3,
+	ccu_hps_mem_region_memspace1c = 4,
+	ccu_hps_mem_region_memspace1d = 5,
+	ccu_hps_mem_region_memspace1e = 6,
+} ccu_hps_mem_region_t;
+
+// Disables mem0 (DDR) region with the given ID.
+int ccu_hps_mem0_region_disable(int id);
+
+// Enables mem0 (DDR) region with the given ID.
+int ccu_hps_mem0_region_enable(int id);
+
+// Attempts to remap mem0 (DDR) region with the given ID to span the given
+// start and end address. It also assigns the security nad privlege policy.
+// Regions must be a power-of-two in size with a minimum size of 64B.
+int ccu_hps_mem0_region_remap(int id, uintptr_t start, uintptr_t end,
+CCU_REGION_SECURITY_t security, CCU_REGION_PRIVILEGE_t privilege);
+
+// Verifies that all enabled mem0 (DDR) regions does not overlap.
+int ccu_hps_mem0_validate(void);
+
+typedef enum ccu_hps_ios_region_e {
+	ccu_hps_ios_region_iospace0a = 0,
+	ccu_hps_ios_region_iospace0b = 1,
+	ccu_hps_ios_region_iospace1a = 2,
+	ccu_hps_ios_region_iospace1b = 3,
+	ccu_hps_ios_region_iospace1c = 4,
+	ccu_hps_ios_region_iospace1d = 5,
+	ccu_hps_ios_region_iospace1e = 6,
+	ccu_hps_ios_region_iospace1f = 7,
+	ccu_hps_ios_region_iospace1g = 8,
+	ccu_hps_ios_region_iospace2a = 9,
+	ccu_hps_ios_region_iospace2b = 10,
+	ccu_hps_ios_region_iospace2c = 11,
+} ccu_hps_ios_region_t;
+
+// Disables the IOS (IO Slave) region with the given ID.
+int ccu_hps_ios_region_disable(int id);
+
+// Enables the IOS (IO Slave) region with the given ID.
+int ccu_hps_ios_region_enable(int id);
+
+
+#define NCORE_CCU_OFFSET			0xf7000000
+
+/* Coherent Sub-System Address Map */
+#define NCORE_CAIU_OFFSET			0x00000
+#define NCORE_CAIU_SIZE				0x01000
+#define NCORE_NCBU_OFFSET			0x60000
+#define NCORE_NCBU_SIZE				0x01000
+#define NCORE_DIRU_OFFSET			0x80000
+#define NCORE_DIRU_SIZE				0x01000
+#define NCORE_CMIU_OFFSET			0xc0000
+#define NCORE_CMIU_SIZE				0x01000
+#define NCORE_CSR_OFFSET			0xff000
+#define NCORE_CSADSERO				0x00040
+#define NCORE_CSUIDR				0x00ff8
+#define NCORE_CSIDR				0x00ffc
+/* Directory Unit Register Map */
+#define NCORE_DIRUSFER				0x00010
+#define NCORE_DIRUMRHER				0x00070
+#define NCORE_DIRUSFMCR				0x00080
+#define NCORE_DIRUSFMAR				0x00084
+/* Coherent Agent Interface Unit Register Map */
+#define NCORE_CAIUIDR				0x00ffc
+/* Snoop Enable Register */
+#define NCORE_DIRUCASER0			0x00040
+#define NCORE_DIRUCASER1			0x00044
+#define NCORE_DIRUCASER2			0x00048
+#define NCORE_DIRUCASER3			0x0004c
+#define NCORE_CSADSER0				0x00040
+#define NCORE_CSADSER1				0x00044
+#define NCORE_CSADSER2				0x00048
+#define NCORE_CSADSER3				0x0004c
+/* Protocols Definition */
+#define ACE_W_DVM				0
+#define ACE_L_W_DVM				1
+#define ACE_WO_DVM				2
+#define ACE_L_WO_DVM				3
+/* Bypass OC Ram Firewall */
+#define NCORE_FW_OCRAM_BLK_BASE			0x100200
+#define NCORE_FW_OCRAM_BLK_CGF1			0x04
+#define NCORE_FW_OCRAM_BLK_CGF2			0x08
+#define NCORE_FW_OCRAM_BLK_CGF3			0x0c
+#define NCORE_FW_OCRAM_BLK_CGF4			0x10
+#define OCRAM_PRIVILEGED_MASK			BIT(29)
+#define OCRAM_SECURE_MASK			BIT(30)
+/* Macros */
+#define NCORE_CCU_REG(base)			(NCORE_CCU_OFFSET + (base))
+#define NCORE_CCU_CSR(reg)			(NCORE_CCU_REG(NCORE_CSR_OFFSET)\
+						+ (reg))
+#define NCORE_CCU_DIR(reg)			(NCORE_CCU_REG(NCORE_DIRU_OFFSET)\
+						+ (reg))
+#define NCORE_CCU_CAI(reg)			(NCORE_CCU_REG(NCORE_CAIU_OFFSET)\
+						+ (reg))
+#define DIRECTORY_UNIT(x, reg)			(NCORE_CCU_DIR(reg)\
+						+ NCORE_DIRU_SIZE * (x))
+#define COH_AGENT_UNIT(x, reg)			(NCORE_CCU_CAI(reg)\
+						+ NCORE_CAIU_SIZE * (x))
+#define COH_CPU0_BYPASS_REG(reg)		(NCORE_CCU_REG(NCORE_FW_OCRAM_BLK_BASE)\
+						+ (reg))
+#define CSUIDR_NUM_CMI(x)			(((x) & 0x3f000000) >> 24)
+#define CSUIDR_NUM_DIR(x)			(((x) & 0x003f0000) >> 16)
+#define CSUIDR_NUM_NCB(x)			(((x) & 0x00003f00) >> 8)
+#define CSUIDR_NUM_CAI(x)			(((x) & 0x0000007f) >> 0)
+#define CSIDR_NUM_SF(x)				(((x) & 0x007c0000) >> 18)
+#define SNOOP_FILTER_ID(x)			(((x) << 16))
+#define CACHING_AGENT_BIT(x)			(((x) & 0x08000) >> 15)
+#define CACHING_AGENT_TYPE(x)			(((x) & 0xf0000) >> 16)
+
+typedef struct coh_ss_id {
+	uint8_t num_coh_mem;
+	uint8_t num_directory;
+	uint8_t num_non_coh_bridge;
+	uint8_t num_coh_agent;
+	uint8_t num_snoop_filter;
+} coh_ss_id_t;
+
+uint32_t init_ncore_ccu(void);
+void ncore_enable_ocram_firewall(void);
+void setup_smmu_stream_id(void);
+
+#endif
diff --git a/plat/intel/soc/common/drivers/combophy/combophy.c b/plat/intel/soc/common/drivers/combophy/combophy.c
new file mode 100644
index 0000000..6c53bc1
--- /dev/null
+++ b/plat/intel/soc/common/drivers/combophy/combophy.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/cadence/cdns_sdmmc.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+
+#include "combophy.h"
+#include "sdmmc/sdmmc.h"
+
+/* Temp assigned handoff data, need to remove when SDM up and run. */
+void config_nand(handoff *hoff_ptr)
+{
+	/* This is hardcoded input value for Combo PHY and SD host controller. */
+	hoff_ptr->peripheral_pwr_gate_array = 0x40;
+
+}
+
+/* DFI configuration */
+int dfi_select(handoff *hoff_ptr)
+{
+	uint32_t data = 0;
+
+	/* Temp assigned handoff data, need to remove when SDM up and run. */
+	handoff reverse_handoff_ptr;
+
+	/* Temp assigned handoff data, need to remove when SDM up and run. */
+	config_nand(&reverse_handoff_ptr);
+
+	if (((reverse_handoff_ptr.peripheral_pwr_gate_array) & PERIPHERAL_SDMMC_MASK) == 0U) {
+		ERROR("SDMMC/NAND is not set properly\n");
+		return -ENXIO;
+	}
+
+	mmio_setbits_32(SOCFPGA_SYSMGR(DFI_INTF),
+		(((reverse_handoff_ptr.peripheral_pwr_gate_array) &
+		PERIPHERAL_SDMMC_MASK) >> PERIPHERAL_SDMMC_OFFSET));
+	data = mmio_read_32(SOCFPGA_SYSMGR(DFI_INTF));
+	if ((data & DFI_INTF_MASK) != (((reverse_handoff_ptr.peripheral_pwr_gate_array) &
+		PERIPHERAL_SDMMC_MASK) >> PERIPHERAL_SDMMC_OFFSET)) {
+		ERROR("DFI is not set properly\n");
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+int combo_phy_init(handoff *hoff_ptr)
+{
+	/* SDMMC/NAND DFI selection based on system manager DFI register */
+	int ret = dfi_select(hoff_ptr);
+
+	if (ret != 0U) {
+		ERROR("DFI configuration failed\n");
+		return ret;
+	}
+
+	return 0;
+}
diff --git a/plat/intel/soc/common/drivers/combophy/combophy.h b/plat/intel/soc/common/drivers/combophy/combophy.h
new file mode 100644
index 0000000..ca571f7
--- /dev/null
+++ b/plat/intel/soc/common/drivers/combophy/combophy.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef COMBOPHY_H
+#define COMBOPHY_H
+
+#include <lib/mmio.h>
+
+#include "socfpga_handoff.h"
+
+#define PERIPHERAL_SDMMC_MASK		0x60
+#define PERIPHERAL_SDMMC_OFFSET		6
+#define DFI_INTF_MASK			0x1
+
+/* FUNCTION DEFINATION */
+/*
+ * @brief Nand controller initialization function
+ *
+ * @hoff_ptr: Pointer to the hand-off data
+ * Return: 0 on success, a negative errno on failure
+ */
+int combo_phy_init(handoff *hoff_ptr);
+int dfi_select(handoff *hoff_ptr);
+
+#endif
diff --git a/plat/intel/soc/common/drivers/ddr/ddr.c b/plat/intel/soc/common/drivers/ddr/ddr.c
new file mode 100644
index 0000000..188302f
--- /dev/null
+++ b/plat/intel/soc/common/drivers/ddr/ddr.c
@@ -0,0 +1,342 @@
+/*
+ * Copyright (c) 2022, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <common/debug.h>
+#include "ddr.h"
+#include <lib/mmio.h>
+#include "socfpga_handoff.h"
+
+int ddr_calibration_check(void)
+{
+	// DDR calibration check
+	int status = 0;
+	uint32_t u32data_read = 0;
+
+	NOTICE("DDR: Access address 0x%x:...\n", IO96B_0_REG_BASE);
+	u32data_read = mmio_read_32(IO96B_0_REG_BASE);
+	NOTICE("DDR: Access address 0x%x: read 0x%04x\n", IO96B_0_REG_BASE, u32data_read);
+
+	if (u32data_read == -EPERM) {
+		status = -EPERM;
+		assert(u32data_read);
+	}
+
+	u32data_read = 0x0;
+	NOTICE("DDR: Access address 0x%x: ...\n", IO96B_1_REG_BASE);
+	u32data_read = mmio_read_32(IO96B_1_REG_BASE);
+	NOTICE("DDR: Access address 0x%x: read 0x%04x\n", IO96B_1_REG_BASE, u32data_read);
+
+	if (u32data_read == -EPERM) {
+		status = -EPERM;
+		assert(u32data_read);
+	}
+
+	return status;
+}
+
+int iossm_mb_init(void)
+{
+	// int status;
+
+	// Update according to IOSSM mailbox spec
+
+	// if (status) {
+		// return status;
+	// }
+
+	return 0;
+}
+
+int wait_respond(uint16_t timeout)
+{
+	uint32_t status = 0;
+	uint32_t count = 0;
+	uint32_t data = 0;
+
+	/* Wait status command response ready */
+	do {
+		data = mmio_read_32(IO96B_CSR_REG(CMD_RESPONSE_STATUS));
+		count++;
+		if (count >= timeout) {
+			return -ETIMEDOUT;
+		}
+
+	} while (STATUS_COMMAND_RESPONSE(data) != STATUS_COMMAND_RESPONSE_READY);
+
+	status = (data & STATUS_GENERAL_ERROR_MASK) >> STATUS_GENERAL_ERROR_OFFSET;
+	if (status != 0) {
+		return status;
+	}
+
+	status = (data & STATUS_CMD_RESPONSE_ERROR_MASK) >> STATUS_CMD_RESPONSE_ERROR_OFFSET;
+	if (status != 0) {
+		return status;
+	}
+
+	return status;
+}
+
+int iossm_mb_read_response(void)
+{
+	uint32_t status = 0;
+	unsigned int i;
+	uint32_t resp_data[IOSSM_RESP_MAX_WORD_SIZE];
+	uint32_t resp_param_reg;
+
+	// Check STATUS_CMD_RESPONSE_DATA_PTR_VALID in
+	// STATUS_COMMAND_RESPONSE to ensure data pointer response
+
+	/* Read CMD_RESPONSE_STATUS and CMD_RESPONSE_DATA_* */
+	resp_data[0] = mmio_read_32(IO96B_CSR_REG(CMD_RESPONSE_STATUS));
+	resp_data[0] = (resp_data[0] & CMD_RESPONSE_DATA_SHORT_MASK) >>
+			CMD_RESPONSE_DATA_SHORT_OFFSET;
+	resp_param_reg = CMD_RESPONSE_STATUS;
+	for (i = 1; i < IOSSM_RESP_MAX_WORD_SIZE; i++) {
+		resp_param_reg = resp_param_reg - CMD_RESPONSE_OFFSET;
+		resp_data[i] = mmio_read_32(IO96B_CSR_REG(resp_param_reg));
+	}
+
+	/* Wait for STATUS_COMMAND_RESPONSE_READY*/
+	status = wait_respond(1000);
+
+	/* Read CMD_RESPONSE_STATUS and CMD_RESPONSE_DATA_* */
+	mmio_setbits_32(STATUS_COMMAND_RESPONSE(IO96B_CSR_REG(
+			CMD_RESPONSE_STATUS)),
+			STATUS_COMMAND_RESPONSE_READY_CLEAR);
+
+	return status;
+}
+
+int iossm_mb_send(uint32_t cmd_target_ip_type, uint32_t cmd_target_ip_instance_id,
+			uint32_t cmd_type, uint32_t cmd_opcode, uint32_t *args,
+			unsigned int len)
+{
+	unsigned int i;
+	uint32_t status = 0;
+	uint32_t cmd_req;
+	uint32_t cmd_param_reg;
+
+	cmd_target_ip_type = (cmd_target_ip_type & CMD_TARGET_IP_TYPE_MASK) <<
+				CMD_TARGET_IP_TYPE_OFFSET;
+	cmd_target_ip_instance_id = (cmd_target_ip_instance_id &
+				CMD_TARGET_IP_INSTANCE_ID_MASK) <<
+				CMD_TARGET_IP_INSTANCE_ID_OFFSET;
+	cmd_type = (cmd_type & CMD_TYPE_MASK) << CMD_TYPE_OFFSET;
+	cmd_opcode = (cmd_opcode & CMD_OPCODE_MASK) << CMD_OPCODE_OFFSET;
+	cmd_req = cmd_target_ip_type | cmd_target_ip_instance_id | cmd_type |
+			cmd_opcode;
+
+	/* send mailbox request */
+	IOSSM_MB_WRITE(IO96B_CSR_REG(CMD_REQ), cmd_req);
+	if (len != 0) {
+		cmd_param_reg = CMD_REQ;
+		for (i = 0; i < len; i++) {
+			cmd_param_reg = cmd_param_reg - CMD_PARAM_OFFSET;
+			IOSSM_MB_WRITE(IO96B_CSR_REG(cmd_param_reg), args[i]);
+		}
+	}
+
+	status = iossm_mb_read_response();
+	if (status != 0) {
+		return status;
+	}
+
+	return status;
+}
+
+int ddr_iossm_mailbox_cmd(uint32_t cmd_opcode)
+{
+	// IOSSM
+	uint32_t status = 0;
+	unsigned int i = 0;
+	uint32_t payload[IOSSM_CMD_MAX_WORD_SIZE] = {0U};
+
+	switch (cmd_opcode) {
+	case CMD_INIT:
+		status = iossm_mb_init();
+		break;
+
+	case OPCODE_GET_MEM_INTF_INFO:
+		status = iossm_mb_send(0, 0, MBOX_CMD_GET_SYS_INFO,
+		OPCODE_GET_MEM_INTF_INFO, payload, i);
+		break;
+
+	case OPCODE_GET_MEM_TECHNOLOGY:
+		status = iossm_mb_send(0, 0, MBOX_CMD_GET_MEM_INFO,
+		OPCODE_GET_MEM_TECHNOLOGY, payload, i);
+		break;
+
+	case OPCODE_GET_MEM_WIDTH_INFO:
+		status = iossm_mb_send(0, 0, MBOX_CMD_GET_MEM_INFO,
+		OPCODE_GET_MEM_WIDTH_INFO, payload, i);
+		break;
+
+	case OPCODE_ECC_ENABLE_STATUS:
+		status = iossm_mb_send(0, 0,
+		MBOX_CMD_TRIG_CONTROLLER_OP, OPCODE_ECC_ENABLE_STATUS,
+		payload, i);
+		break;
+
+	case OPCODE_ECC_INTERRUPT_MASK:
+		// payload[i] = CMD_PARAM_0 [16:0]: ECC_INTERRUPT_MASK
+		status = iossm_mb_send(0, 0,
+		MBOX_CMD_TRIG_CONTROLLER_OP, OPCODE_ECC_INTERRUPT_MASK,
+		payload, i);
+		break;
+
+	case OPCODE_ECC_SCRUB_MODE_0_START:
+		// payload[i] = CMD_PARAM_0 [15:0]: ECC_SCRUB_INTERVAL
+		//i++;
+		// payload[i] = CMD_PARAM_1 [11:0]: ECC_SCRUB_LEN
+		//i++;
+		// payload[i] = CMD_PARAM_2 [0:0]: ECC_SCRUB_FULL_MEM
+		//i++;
+		// payload[i]= CMD_PARAM_3 [31:0]: ECC_SCRUB_START_ADDR [31:0]
+		//i++;
+		// payload[i] = CMD_PARAM_4 [5:0]: ECC_SCRUB_START_ADDR [36:32]
+		//i++;
+		// payload[i] = CMD_PARAM_5 [31:0]: ECC_SCRUB_END_ADDR [31:0]
+		//i++;
+		// payload[i] = CMD_PARAM_6 [5:0]: ECC_SCRUB_END_ADDR [36:32]
+		//i++;
+		status = iossm_mb_send(0, 0,
+		MBOX_CMD_TRIG_CONTROLLER_OP, OPCODE_ECC_SCRUB_MODE_0_START,
+		payload, i);
+		break;
+
+	case OPCODE_ECC_SCRUB_MODE_1_START:
+		// payload[i] = CMD_PARAM_0 [15:0]: ECC_SCRUB_IDLE_CNT
+		//i++;
+		// payload[i] = CMD_PARAM_1 [11:0]: ECC_SCRUB_LEN
+		//i++;
+		// payload[i] = CMD_PARAM_2 [0:0]: ECC_SCRUB_FULL_MEM
+		//i++;
+		// payload[i] = CMD_PARAM_3 [31:0]: ECC_SCRUB_START_ADDR [31:0]
+		//i++;
+		// payload[i] = CMD_PARAM_4 [5:0]: ECC_SCRUB_START_ADDR [36:32]
+		//i++;
+		// payload[i] = CMD_PARAM_5 [31:0]: ECC_SCRUB_END_ADDR [31:0]
+		//i++;
+		// payload[i] = CMD_PARAM_6 [5:0]: ECC_SCRUB_END_ADDR [36:32]
+		//i++;
+		status = iossm_mb_send(0, 0,
+		MBOX_CMD_TRIG_CONTROLLER_OP, OPCODE_ECC_SCRUB_MODE_1_START,
+		payload, i);
+		break;
+
+	case OPCODE_BIST_RESULTS_STATUS:
+		status = iossm_mb_send(0, 0,
+		MBOX_CMD_TRIG_CONTROLLER_OP, OPCODE_BIST_RESULTS_STATUS,
+		payload, i);
+		break;
+
+	case OPCODE_BIST_MEM_INIT_START:
+		status = iossm_mb_send(0, 0,
+		MBOX_CMD_TRIG_CONTROLLER_OP, OPCODE_BIST_MEM_INIT_START,
+		payload, i);
+		break;
+
+	case OPCODE_TRIG_MEM_CAL:
+		status = iossm_mb_send(0, 0, MBOX_CMD_TRIG_MEM_CAL_OP,
+		OPCODE_TRIG_MEM_CAL, payload, i);
+		break;
+
+	default:
+		break;
+	}
+
+	if (status == -EPERM) {
+		assert(status);
+	}
+
+	return status;
+}
+
+int ddr_config_handoff(handoff *hoff_ptr)
+{
+	/* Populate DDR handoff data */
+	/* TODO: To add in DDR handoff configuration once available */
+	return 0;
+}
+
+// DDR firewall and non secure access
+void ddr_enable_ns_access(void)
+{
+	/* Please set the ddr non secure registers accordingly */
+
+	mmio_setbits_32(CCU_REG(DMI0_DMIUSMCTCR),
+			CCU_DMI_ALLOCEN | CCU_DMI_LOOKUPEN);
+	mmio_setbits_32(CCU_REG(DMI1_DMIUSMCTCR),
+			CCU_DMI_ALLOCEN | CCU_DMI_LOOKUPEN);
+
+	/* TODO: To add in CCU NCORE OCRAM bypass mask for non secure registers */
+	NOTICE("DDR non secure configured\n");
+}
+
+void ddr_enable_firewall(void)
+{
+	/* Please set the ddr firewall registers accordingly */
+	/* TODO: To add in CCU NCORE OCRAM bypass mask for firewall registers */
+	NOTICE("DDR firewall enabled\n");
+}
+
+bool is_ddr_init_in_progress(void)
+{
+	uint32_t reg = mmio_read_32(SOCFPGA_SYSMGR(BOOT_SCRATCH_POR_0));
+
+	if (reg & SOCFPGA_SYSMGR_BOOT_SCRATCH_POR_0_MASK) {
+		return true;
+	}
+	return false;
+}
+
+int ddr_init(void)
+{
+	// DDR driver initialization
+	int status = -EPERM;
+	uint32_t cmd_opcode = 0;
+
+	// Check and set Boot Scratch Register
+	if (is_ddr_init_in_progress()) {
+		return status;
+	}
+	mmio_write_32(SOCFPGA_SYSMGR(BOOT_SCRATCH_POR_0), 0x01);
+
+	// Populate DDR handoff data
+	handoff reverse_handoff_ptr;
+
+	if (!socfpga_get_handoff(&reverse_handoff_ptr)) {
+		assert(status);
+	}
+	status = ddr_config_handoff(&reverse_handoff_ptr);
+	if (status == -EPERM) {
+		assert(status);
+	}
+
+	// CCU and firewall setup
+	ddr_enable_ns_access();
+	ddr_enable_firewall();
+
+	// DDR calibration check
+	status = ddr_calibration_check();
+	if (status == -EPERM) {
+		assert(status);
+	}
+
+	// DDR mailbox command
+	status = ddr_iossm_mailbox_cmd(cmd_opcode);
+	if (status != 0) {
+		assert(status);
+	}
+
+	// Check and set Boot Scratch Register
+	mmio_write_32(SOCFPGA_SYSMGR(BOOT_SCRATCH_POR_0), 0x00);
+
+	NOTICE("DDR init successfully\n");
+	return status;
+}
diff --git a/plat/intel/soc/common/drivers/ddr/ddr.h b/plat/intel/soc/common/drivers/ddr/ddr.h
new file mode 100644
index 0000000..416b64e
--- /dev/null
+++ b/plat/intel/soc/common/drivers/ddr/ddr.h
@@ -0,0 +1,112 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef DDR_H
+#define DDR_H
+
+#include <lib/mmio.h>
+#include "socfpga_handoff.h"
+
+/* MACRO DEFINATION */
+#define IO96B_0_REG_BASE				0x18400000
+#define IO96B_1_REG_BASE				0x18800000
+#define IO96B_CSR_BASE					0x05000000
+#define IO96B_CSR_REG(reg)				(IO96B_CSR_BASE + reg)
+
+#define IOSSM_CMD_MAX_WORD_SIZE				7U
+#define IOSSM_RESP_MAX_WORD_SIZE			4U
+
+#define CCU_REG_BASE					0x1C000000
+#define DMI0_DMIUSMCTCR					0x7300
+#define DMI1_DMIUSMCTCR					0x8300
+#define CCU_DMI_ALLOCEN					BIT(1)
+#define CCU_DMI_LOOKUPEN				BIT(2)
+#define CCU_REG(reg)					(CCU_REG_BASE + reg)
+
+// CMD_RESPONSE_STATUS Register
+#define CMD_RESPONSE_STATUS				0x45C
+#define CMD_RESPONSE_OFFSET				0x4
+#define CMD_RESPONSE_DATA_SHORT_MASK			GENMASK(31, 16)
+#define CMD_RESPONSE_DATA_SHORT_OFFSET			16
+#define STATUS_CMD_RESPONSE_ERROR_MASK			GENMASK(7, 5)
+#define STATUS_CMD_RESPONSE_ERROR_OFFSET		5
+#define STATUS_GENERAL_ERROR_MASK			GENMASK(4, 1)
+#define STATUS_GENERAL_ERROR_OFFSET			1
+#define STATUS_COMMAND_RESPONSE_READY			0x1
+#define STATUS_COMMAND_RESPONSE_READY_CLEAR		0x0
+#define STATUS_COMMAND_RESPONSE_READY_MASK		0x1
+#define STATUS_COMMAND_RESPONSE_READY_OFFSET		0
+#define STATUS_COMMAND_RESPONSE(x)			(((x) & \
+							STATUS_COMMAND_RESPONSE_READY_MASK) >> \
+							STATUS_COMMAND_RESPONSE_READY_OFFSET)
+
+// CMD_REQ Register
+#define CMD_STATUS					0x400
+#define CMD_PARAM					0x438
+#define CMD_REQ						0x43C
+#define CMD_PARAM_OFFSET				0x4
+#define CMD_TARGET_IP_TYPE_MASK				GENMASK(31, 29)
+#define CMD_TARGET_IP_TYPE_OFFSET			29
+#define CMD_TARGET_IP_INSTANCE_ID_MASK			GENMASK(28, 24)
+#define CMD_TARGET_IP_INSTANCE_ID_OFFSET		24
+#define CMD_TYPE_MASK					GENMASK(23, 16)
+#define CMD_TYPE_OFFSET					16
+#define CMD_OPCODE_MASK					GENMASK(15, 0)
+#define CMD_OPCODE_OFFSET				0
+
+#define CMD_INIT					0
+
+#define OPCODE_GET_MEM_INTF_INFO			0x0001
+#define OPCODE_GET_MEM_TECHNOLOGY			0x0002
+#define OPCODE_GET_MEM_WIDTH_INFO			0x0004
+#define OPCODE_TRIG_MEM_CAL				0x000A
+#define OPCODE_ECC_ENABLE_STATUS			0x0102
+#define OPCODE_ECC_INTERRUPT_MASK			0x0105
+#define OPCODE_ECC_SCRUB_MODE_0_START			0x0202
+#define OPCODE_ECC_SCRUB_MODE_1_START			0x0203
+#define OPCODE_BIST_RESULTS_STATUS			0x0302
+#define OPCODE_BIST_MEM_INIT_START			0x0303
+// Please update according to IOSSM mailbox spec
+#define MBOX_ID_IOSSM					0x00
+#define MBOX_CMD_GET_SYS_INFO				0x01
+// Please update according to IOSSM mailbox spec
+#define MBOX_CMD_GET_MEM_INFO				0x02
+#define MBOX_CMD_TRIG_CONTROLLER_OP			0x04
+#define MBOX_CMD_TRIG_MEM_CAL_OP			0x05
+#define MBOX_CMD_POKE_REG				0xFD
+#define MBOX_CMD_PEEK_REG				0xFE
+#define MBOX_CMD_GET_DEBUG_LOG				0xFF
+// Please update according to IOSSM mailbox spec
+#define MBOX_CMD_DIRECT					0x00
+
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_POR_0_MASK		0x01
+
+#define IOSSM_MB_WRITE(addr, data)			mmio_write_32(addr, data)
+
+/* FUNCTION DEFINATION */
+int ddr_calibration_check(void);
+
+int iossm_mb_init(void);
+
+int iossm_mb_read_response(void);
+
+int iossm_mb_send(uint32_t cmd_target_ip_type, uint32_t cmd_target_ip_instance_id,
+			uint32_t cmd_type, uint32_t cmd_opcode, uint32_t *args,
+			unsigned int len);
+
+int ddr_iossm_mailbox_cmd(uint32_t cmd);
+
+int ddr_init(void);
+
+int ddr_config_handoff(handoff *hoff_ptr);
+
+void ddr_enable_ns_access(void);
+
+void ddr_enable_firewall(void);
+
+bool is_ddr_init_in_progress(void);
+
+#endif
diff --git a/plat/intel/soc/common/drivers/nand/nand.c b/plat/intel/soc/common/drivers/nand/nand.c
new file mode 100644
index 0000000..c6acbe3
--- /dev/null
+++ b/plat/intel/soc/common/drivers/nand/nand.c
@@ -0,0 +1,57 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/cadence/cdns_nand.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include "nand.h"
+
+#include "agilex5_pinmux.h"
+#include "combophy/combophy.h"
+
+/* Pinmux configuration */
+static void nand_pinmux_config(void)
+{
+	mmio_write_32(SOCFPGA_PINMUX(PIN0SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN1SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN2SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN3SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN4SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN5SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN6SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN7SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN8SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN9SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN10SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN11SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN12SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN13SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN14SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN16SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN17SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN18SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN19SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN20SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN21SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN22SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN23SEL), SOCFPGA_PINMUX_SEL_NAND);
+}
+
+int nand_init(handoff *hoff_ptr)
+{
+	/* NAND pin mux configuration */
+	nand_pinmux_config();
+
+	return cdns_nand_host_init();
+}
diff --git a/plat/intel/soc/common/drivers/nand/nand.h b/plat/intel/soc/common/drivers/nand/nand.h
new file mode 100644
index 0000000..b060a72
--- /dev/null
+++ b/plat/intel/soc/common/drivers/nand/nand.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef DDR_H
+#define DDR_H
+
+#include <lib/mmio.h>
+#include "socfpga_handoff.h"
+
+/* FUNCTION DEFINATION */
+/*
+ * @brief Nand controller initialization function
+ *
+ * @hoff_ptr: Pointer to the hand-off data
+ * Return: 0 on success, a negative errno on failure
+ */
+int nand_init(handoff *hoff_ptr);
+
+#endif
diff --git a/plat/intel/soc/common/drivers/qspi/cadence_qspi.c b/plat/intel/soc/common/drivers/qspi/cadence_qspi.c
new file mode 100644
index 0000000..da8a8bd
--- /dev/null
+++ b/plat/intel/soc/common/drivers/qspi/cadence_qspi.c
@@ -0,0 +1,823 @@
+/*
+ * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <string.h>
+#include <drivers/delay_timer.h>
+#include <drivers/console.h>
+
+#include "cadence_qspi.h"
+#include "socfpga_plat_def.h"
+
+#define LESS(a, b)   (((a) < (b)) ? (a) : (b))
+#define MORE(a, b)   (((a) > (b)) ? (a) : (b))
+
+
+uint32_t qspi_device_size;
+int cad_qspi_cs;
+
+int cad_qspi_idle(void)
+{
+	return (mmio_read_32(CAD_QSPI_OFFSET + CAD_QSPI_CFG)
+			& CAD_QSPI_CFG_IDLE) >> 31;
+}
+
+int cad_qspi_set_baudrate_div(uint32_t div)
+{
+	if (div > 0xf)
+		return CAD_INVALID;
+
+	mmio_clrsetbits_32(CAD_QSPI_OFFSET + CAD_QSPI_CFG,
+			~CAD_QSPI_CFG_BAUDDIV_MSK,
+			CAD_QSPI_CFG_BAUDDIV(div));
+
+	return 0;
+}
+
+int cad_qspi_configure_dev_size(uint32_t addr_bytes,
+		uint32_t bytes_per_dev, uint32_t bytes_per_block)
+{
+
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_DEVSZ,
+			CAD_QSPI_DEVSZ_ADDR_BYTES(addr_bytes) |
+			CAD_QSPI_DEVSZ_BYTES_PER_PAGE(bytes_per_dev) |
+			CAD_QSPI_DEVSZ_BYTES_PER_BLOCK(bytes_per_block));
+	return 0;
+}
+
+int cad_qspi_set_read_config(uint32_t opcode, uint32_t instr_type,
+		uint32_t addr_type, uint32_t data_type,
+		uint32_t mode_bit, uint32_t dummy_clk_cycle)
+{
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_DEVRD,
+			CAD_QSPI_DEV_OPCODE(opcode) |
+			CAD_QSPI_DEV_INST_TYPE(instr_type) |
+			CAD_QSPI_DEV_ADDR_TYPE(addr_type) |
+			CAD_QSPI_DEV_DATA_TYPE(data_type) |
+			CAD_QSPI_DEV_MODE_BIT(mode_bit) |
+			CAD_QSPI_DEV_DUMMY_CLK_CYCLE(dummy_clk_cycle));
+
+	return 0;
+}
+
+int cad_qspi_set_write_config(uint32_t opcode, uint32_t addr_type,
+		uint32_t data_type, uint32_t dummy_clk_cycle)
+{
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_DEVWR,
+			CAD_QSPI_DEV_OPCODE(opcode) |
+			CAD_QSPI_DEV_ADDR_TYPE(addr_type) |
+			CAD_QSPI_DEV_DATA_TYPE(data_type) |
+			CAD_QSPI_DEV_DUMMY_CLK_CYCLE(dummy_clk_cycle));
+
+	return 0;
+}
+
+int cad_qspi_timing_config(uint32_t clkphase, uint32_t clkpol, uint32_t csda,
+		uint32_t csdads, uint32_t cseot, uint32_t cssot,
+		uint32_t rddatacap)
+{
+	uint32_t cfg = mmio_read_32(CAD_QSPI_OFFSET + CAD_QSPI_CFG);
+
+	cfg &= CAD_QSPI_CFG_SELCLKPHASE_CLR_MSK &
+		CAD_QSPI_CFG_SELCLKPOL_CLR_MSK;
+	cfg |= CAD_QSPI_SELCLKPHASE(clkphase) | CAD_QSPI_SELCLKPOL(clkpol);
+
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_CFG, cfg);
+
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_DELAY,
+		CAD_QSPI_DELAY_CSSOT(cssot) | CAD_QSPI_DELAY_CSEOT(cseot) |
+		CAD_QSPI_DELAY_CSDADS(csdads) | CAD_QSPI_DELAY_CSDA(csda));
+
+	return 0;
+}
+
+int cad_qspi_stig_cmd_helper(int cs, uint32_t cmd)
+{
+	uint32_t count = 0;
+
+	/* chip select */
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_CFG,
+			(mmio_read_32(CAD_QSPI_OFFSET + CAD_QSPI_CFG)
+			 & CAD_QSPI_CFG_CS_MSK) | CAD_QSPI_CFG_CS(cs));
+
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_FLASHCMD, cmd);
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_FLASHCMD,
+			cmd | CAD_QSPI_FLASHCMD_EXECUTE);
+
+	do {
+		uint32_t reg = mmio_read_32(CAD_QSPI_OFFSET +
+					CAD_QSPI_FLASHCMD);
+		if (!(reg & CAD_QSPI_FLASHCMD_EXECUTE_STAT))
+			break;
+		count++;
+	} while (count < CAD_QSPI_COMMAND_TIMEOUT);
+
+	if (count >= CAD_QSPI_COMMAND_TIMEOUT) {
+		ERROR("Error sending QSPI command %x, timed out\n",
+				cmd);
+		return CAD_QSPI_ERROR;
+	}
+
+	return 0;
+}
+
+int cad_qspi_stig_cmd(uint32_t opcode, uint32_t dummy)
+{
+	if (dummy > ((1 << CAD_QSPI_FLASHCMD_NUM_DUMMYBYTES_MAX) - 1)) {
+		ERROR("Faulty dummy bytes\n");
+		return -1;
+	}
+
+	return cad_qspi_stig_cmd_helper(cad_qspi_cs,
+			CAD_QSPI_FLASHCMD_OPCODE(opcode) |
+			CAD_QSPI_FLASHCMD_NUM_DUMMYBYTES(dummy));
+}
+
+int cad_qspi_stig_read_cmd(uint32_t opcode, uint32_t dummy, uint32_t num_bytes,
+		uint32_t *output)
+{
+	if (dummy > ((1 << CAD_QSPI_FLASHCMD_NUM_DUMMYBYTES_MAX) - 1)) {
+		ERROR("Faulty dummy byes\n");
+		return -1;
+	}
+
+	if ((num_bytes > 8) || (num_bytes == 0))
+		return -1;
+
+	uint32_t cmd =
+		CAD_QSPI_FLASHCMD_OPCODE(opcode) |
+		CAD_QSPI_FLASHCMD_ENRDDATA(1) |
+		CAD_QSPI_FLASHCMD_NUMRDDATABYTES(num_bytes - 1) |
+		CAD_QSPI_FLASHCMD_ENCMDADDR(0) |
+		CAD_QSPI_FLASHCMD_ENMODEBIT(0) |
+		CAD_QSPI_FLASHCMD_NUMADDRBYTES(0) |
+		CAD_QSPI_FLASHCMD_ENWRDATA(0) |
+		CAD_QSPI_FLASHCMD_NUMWRDATABYTES(0) |
+		CAD_QSPI_FLASHCMD_NUMDUMMYBYTES(dummy);
+
+	if (cad_qspi_stig_cmd_helper(cad_qspi_cs, cmd)) {
+		ERROR("failed to send stig cmd\n");
+		return -1;
+	}
+
+	output[0] = mmio_read_32(CAD_QSPI_OFFSET + CAD_QSPI_FLASHCMD_RDDATA0);
+
+	if (num_bytes > 4) {
+		output[1] = mmio_read_32(CAD_QSPI_OFFSET +
+				CAD_QSPI_FLASHCMD_RDDATA1);
+	}
+
+	return 0;
+}
+
+int cad_qspi_stig_wr_cmd(uint32_t opcode, uint32_t dummy, uint32_t num_bytes,
+		uint32_t *input)
+{
+	if (dummy > ((1 << CAD_QSPI_FLASHCMD_NUM_DUMMYBYTES_MAX) - 1)) {
+		ERROR("Faulty dummy byes\n");
+		return -1;
+	}
+
+	if ((num_bytes > 8) || (num_bytes == 0))
+		return -1;
+
+	uint32_t cmd = CAD_QSPI_FLASHCMD_OPCODE(opcode) |
+		CAD_QSPI_FLASHCMD_ENRDDATA(0) |
+		CAD_QSPI_FLASHCMD_NUMRDDATABYTES(0) |
+		CAD_QSPI_FLASHCMD_ENCMDADDR(0) |
+		CAD_QSPI_FLASHCMD_ENMODEBIT(0) |
+		CAD_QSPI_FLASHCMD_NUMADDRBYTES(0) |
+		CAD_QSPI_FLASHCMD_ENWRDATA(1) |
+		CAD_QSPI_FLASHCMD_NUMWRDATABYTES(num_bytes - 1) |
+		CAD_QSPI_FLASHCMD_NUMDUMMYBYTES(dummy);
+
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_FLASHCMD_WRDATA0, input[0]);
+
+	if (num_bytes > 4)
+		mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_FLASHCMD_WRDATA1,
+				input[1]);
+
+	return cad_qspi_stig_cmd_helper(cad_qspi_cs, cmd);
+}
+
+int cad_qspi_stig_addr_cmd(uint32_t opcode, uint32_t dummy, uint32_t addr)
+{
+	uint32_t cmd;
+
+	if (dummy > ((1 << CAD_QSPI_FLASHCMD_NUM_DUMMYBYTES_MAX) - 1))
+		return -1;
+
+	cmd = CAD_QSPI_FLASHCMD_OPCODE(opcode) |
+		CAD_QSPI_FLASHCMD_NUMDUMMYBYTES(dummy) |
+		CAD_QSPI_FLASHCMD_ENCMDADDR(1) |
+		CAD_QSPI_FLASHCMD_NUMADDRBYTES(2);
+
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_FLASHCMD_ADDR, addr);
+
+	return cad_qspi_stig_cmd_helper(cad_qspi_cs, cmd);
+}
+
+int cad_qspi_device_bank_select(uint32_t bank)
+{
+	int status = 0;
+
+	status = cad_qspi_stig_cmd(CAD_QSPI_STIG_OPCODE_WREN, 0);
+	if (status != 0)
+		return status;
+
+	status = cad_qspi_stig_wr_cmd(CAD_QSPI_STIG_OPCODE_WREN_EXT_REG,
+			0, 1, &bank);
+	if (status != 0)
+		return status;
+
+	return cad_qspi_stig_cmd(CAD_QSPI_STIG_OPCODE_WRDIS, 0);
+}
+
+int cad_qspi_device_status(uint32_t *status)
+{
+	return cad_qspi_stig_read_cmd(CAD_QSPI_STIG_OPCODE_RDSR, 0, 1, status);
+}
+
+#if CAD_QSPI_MICRON_N25Q_SUPPORT
+int cad_qspi_n25q_enable(void)
+{
+	cad_qspi_set_read_config(QSPI_FAST_READ, CAD_QSPI_INST_SINGLE,
+			CAD_QSPI_ADDR_FASTREAD, CAT_QSPI_ADDR_SINGLE_IO, 1,
+			0);
+	cad_qspi_set_write_config(QSPI_WRITE, 0, 0, 0);
+
+	return 0;
+}
+
+int cad_qspi_n25q_wait_for_program_and_erase(int program_only)
+{
+	uint32_t status, flag_sr;
+	int count = 0;
+
+	while (count < CAD_QSPI_COMMAND_TIMEOUT) {
+		status = cad_qspi_device_status(&status);
+		if (status != 0) {
+			ERROR("Error getting device status\n");
+			return -1;
+		}
+		if (!CAD_QSPI_STIG_SR_BUSY(status))
+			break;
+		count++;
+	}
+
+	if (count >= CAD_QSPI_COMMAND_TIMEOUT) {
+		ERROR("Timed out waiting for idle\n");
+		return -1;
+	}
+
+	count = 0;
+
+	while (count < CAD_QSPI_COMMAND_TIMEOUT) {
+		status = cad_qspi_stig_read_cmd(CAD_QSPI_STIG_OPCODE_RDFLGSR,
+				0, 1, &flag_sr);
+		if (status != 0) {
+			ERROR("Error waiting program and erase.\n");
+			return status;
+		}
+
+		if ((program_only &&
+			CAD_QSPI_STIG_FLAGSR_PROGRAMREADY(flag_sr)) ||
+			(!program_only &&
+			CAD_QSPI_STIG_FLAGSR_ERASEREADY(flag_sr)))
+			break;
+	}
+
+	if (count >= CAD_QSPI_COMMAND_TIMEOUT)
+		ERROR("Timed out waiting for program and erase\n");
+
+	if ((program_only && CAD_QSPI_STIG_FLAGSR_PROGRAMERROR(flag_sr)) ||
+			(!program_only &&
+			CAD_QSPI_STIG_FLAGSR_ERASEERROR(flag_sr))) {
+		ERROR("Error programming/erasing flash\n");
+		cad_qspi_stig_cmd(CAD_QSPI_STIG_OPCODE_CLFSR, 0);
+		return -1;
+	}
+
+	return 0;
+}
+#endif
+
+int cad_qspi_indirect_read_start_bank(uint32_t flash_addr, uint32_t num_bytes)
+{
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_INDRDSTADDR, flash_addr);
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_INDRDCNT, num_bytes);
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_INDRD,
+			CAD_QSPI_INDRD_START |
+			CAD_QSPI_INDRD_IND_OPS_DONE);
+
+	return 0;
+}
+
+
+int cad_qspi_indirect_write_start_bank(uint32_t flash_addr,
+					uint32_t num_bytes)
+{
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_INDWRSTADDR, flash_addr);
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_INDWRCNT, num_bytes);
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_INDWR,
+			CAD_QSPI_INDWR_START |
+			CAD_QSPI_INDWR_INDDONE);
+
+	return 0;
+}
+
+int cad_qspi_indirect_write_finish(void)
+{
+#if CAD_QSPI_MICRON_N25Q_SUPPORT
+	return cad_qspi_n25q_wait_for_program_and_erase(1);
+#else
+	return 0;
+#endif
+
+}
+
+int cad_qspi_enable(void)
+{
+	int status;
+
+	mmio_setbits_32(CAD_QSPI_OFFSET + CAD_QSPI_CFG, CAD_QSPI_CFG_ENABLE);
+
+#if CAD_QSPI_MICRON_N25Q_SUPPORT
+	status = cad_qspi_n25q_enable();
+	if (status != 0)
+		return status;
+#endif
+	return 0;
+}
+
+int cad_qspi_enable_subsector_bank(uint32_t addr)
+{
+	int status = 0;
+
+	status = cad_qspi_stig_cmd(CAD_QSPI_STIG_OPCODE_WREN, 0);
+	if (status != 0)
+		return status;
+
+	status = cad_qspi_stig_addr_cmd(CAD_QSPI_STIG_OPCODE_SUBSEC_ERASE, 0,
+					addr);
+	if (status != 0)
+		return status;
+
+#if CAD_QSPI_MICRON_N25Q_SUPPORT
+	status = cad_qspi_n25q_wait_for_program_and_erase(0);
+#endif
+	return status;
+}
+
+int cad_qspi_erase_subsector(uint32_t addr)
+{
+	int status = 0;
+
+	status = cad_qspi_device_bank_select(addr >> 24);
+	if (status != 0)
+		return status;
+
+	return cad_qspi_enable_subsector_bank(addr);
+}
+
+int cad_qspi_erase_sector(uint32_t addr)
+{
+	int status = 0;
+
+	status = cad_qspi_device_bank_select(addr >> 24);
+	if (status != 0)
+		return status;
+
+	status = cad_qspi_stig_cmd(CAD_QSPI_STIG_OPCODE_WREN, 0);
+	if (status != 0)
+		return status;
+
+	status = cad_qspi_stig_addr_cmd(CAD_QSPI_STIG_OPCODE_SEC_ERASE, 0,
+					addr);
+	if (status != 0)
+		return status;
+
+#if CAD_QSPI_MICRON_N25Q_SUPPORT
+	status = cad_qspi_n25q_wait_for_program_and_erase(0);
+#endif
+	return status;
+}
+
+void cad_qspi_calibration(uint32_t dev_clk, uint32_t qspi_clk_mhz)
+{
+	int status;
+	uint32_t dev_sclk_mhz = 27; /*min value to get biggest 0xF div factor*/
+	uint32_t data_cap_delay;
+	uint32_t sample_rdid;
+	uint32_t rdid;
+	uint32_t div_actual;
+	uint32_t div_bits;
+	int first_pass, last_pass;
+
+	/*1.  Set divider to bigger value (slowest SCLK)
+	 *2.  RDID and save the value
+	 */
+	div_actual = (qspi_clk_mhz + (dev_sclk_mhz - 1)) / dev_sclk_mhz;
+	div_bits = (((div_actual + 1) / 2) - 1);
+	status = cad_qspi_set_baudrate_div(0xf);
+
+	status = cad_qspi_stig_read_cmd(CAD_QSPI_STIG_OPCODE_RDID,
+					0, 3, &sample_rdid);
+	if (status != 0)
+		return;
+
+	/*3. Set divider to the intended frequency
+	 *4.  Set the read delay = 0
+	 *5.  RDID and check whether the value is same as item 2
+	 *6.  Increase read delay and compared the value against item 2
+	 *7.  Find the range of read delay that have same as
+	 *    item 2 and divide it to 2
+	 */
+	div_actual = (qspi_clk_mhz + (dev_clk - 1)) / dev_clk;
+	div_bits = (((div_actual + 1) / 2) - 1);
+	status = cad_qspi_set_baudrate_div(div_bits);
+	if (status != 0)
+		return;
+
+	data_cap_delay = 0;
+	first_pass = -1;
+	last_pass = -1;
+
+	do {
+		if (status != 0)
+			break;
+		status = cad_qspi_stig_read_cmd(CAD_QSPI_STIG_OPCODE_RDID, 0,
+						3, &rdid);
+		if (status != 0)
+			break;
+		if (rdid == sample_rdid) {
+			if (first_pass == -1)
+				first_pass = data_cap_delay;
+			else
+				last_pass = data_cap_delay;
+		}
+
+		data_cap_delay++;
+
+		mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_RDDATACAP,
+				CAD_QSPI_RDDATACAP_BYP(1) |
+				CAD_QSPI_RDDATACAP_DELAY(data_cap_delay));
+
+	} while (data_cap_delay < 0x10);
+
+	if (first_pass > 0) {
+		int diff = first_pass - last_pass;
+
+		data_cap_delay = first_pass + diff / 2;
+	}
+
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_RDDATACAP,
+			CAD_QSPI_RDDATACAP_BYP(1) |
+			CAD_QSPI_RDDATACAP_DELAY(data_cap_delay));
+	status = cad_qspi_stig_read_cmd(CAD_QSPI_STIG_OPCODE_RDID, 0, 3, &rdid);
+
+	if (status != 0)
+		return;
+}
+
+int cad_qspi_int_disable(uint32_t mask)
+{
+	if (cad_qspi_idle() == 0)
+		return -1;
+
+	if ((CAD_QSPI_INT_STATUS_ALL & mask) == 0)
+		return -1;
+
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_IRQMSK, mask);
+	return 0;
+}
+
+void cad_qspi_set_chip_select(int cs)
+{
+	cad_qspi_cs = cs;
+}
+
+int cad_qspi_init(uint32_t desired_clk_freq, uint32_t clk_phase,
+			uint32_t clk_pol, uint32_t csda, uint32_t csdads,
+			uint32_t cseot, uint32_t cssot, uint32_t rddatacap)
+{
+	int status = 0;
+	uint32_t qspi_desired_clk_freq;
+	uint32_t rdid = 0;
+	uint32_t cap_code;
+
+	INFO("Initializing Qspi\n");
+
+	if (cad_qspi_idle() == 0) {
+		ERROR("device not idle\n");
+		return -1;
+	}
+
+
+	status = cad_qspi_timing_config(clk_phase, clk_pol, csda, csdads,
+					cseot, cssot, rddatacap);
+
+	if (status != 0) {
+		ERROR("config set timing failure\n");
+		return status;
+	}
+
+	mmio_write_32(CAD_QSPI_OFFSET + CAD_QSPI_REMAPADDR,
+			CAD_QSPI_REMAPADDR_VALUE_SET(0));
+
+	status = cad_qspi_int_disable(CAD_QSPI_INT_STATUS_ALL);
+	if (status != 0) {
+		ERROR("failed disable\n");
+		return status;
+	}
+
+	cad_qspi_set_baudrate_div(0xf);
+	status = cad_qspi_enable();
+	if (status != 0) {
+		ERROR("failed enable\n");
+		return status;
+	}
+
+	qspi_desired_clk_freq = 100;
+	cad_qspi_calibration(qspi_desired_clk_freq, 50000000);
+
+	status = cad_qspi_stig_read_cmd(CAD_QSPI_STIG_OPCODE_RDID, 0, 3,
+					&rdid);
+
+	if (status != 0) {
+		ERROR("Error reading RDID\n");
+		return status;
+	}
+
+	/*
+	 * NOTE: The Size code seems to be a form of BCD (binary coded decimal).
+	 * The first nibble is the 10's digit and the second nibble is the 1's
+	 * digit in the number of bytes.
+	 *
+	 * Capacity ID samples:
+	 * 0x15 :   16 Mb =>   2 MiB => 1 << 21 ; BCD=15
+	 * 0x16 :   32 Mb =>   4 MiB => 1 << 22 ; BCD=16
+	 * 0x17 :   64 Mb =>   8 MiB => 1 << 23 ; BCD=17
+	 * 0x18 :  128 Mb =>  16 MiB => 1 << 24 ; BCD=18
+	 * 0x19 :  256 Mb =>  32 MiB => 1 << 25 ; BCD=19
+	 * 0x1a
+	 * 0x1b
+	 * 0x1c
+	 * 0x1d
+	 * 0x1e
+	 * 0x1f
+	 * 0x20 :  512 Mb =>  64 MiB => 1 << 26 ; BCD=20
+	 * 0x21 : 1024 Mb => 128 MiB => 1 << 27 ; BCD=21
+	 */
+
+	cap_code = CAD_QSPI_STIG_RDID_CAPACITYID(rdid);
+
+	if (!(((cap_code >> 4) > 0x9) || ((cap_code & 0xf) > 0x9))) {
+		uint32_t decoded_cap = ((cap_code >> 4) * 10) +
+					(cap_code & 0xf);
+		qspi_device_size = 1 << (decoded_cap + 6);
+		INFO("QSPI Capacity: %x\n\n", qspi_device_size);
+
+	} else {
+		ERROR("Invalid CapacityID encountered: 0x%02x\n",
+				cap_code);
+		return -1;
+	}
+
+	cad_qspi_configure_dev_size(INTEL_QSPI_ADDR_BYTES,
+				INTEL_QSPI_BYTES_PER_DEV,
+				INTEL_BYTES_PER_BLOCK);
+
+	INFO("Flash size: %d Bytes\n", qspi_device_size);
+
+	return status;
+}
+
+int cad_qspi_indirect_page_bound_write(uint32_t offset,
+		uint8_t *buffer, uint32_t len)
+{
+	int status = 0, i;
+	uint32_t write_count, write_capacity, *write_data, space,
+		write_fill_level, sram_partition;
+
+	status = cad_qspi_indirect_write_start_bank(offset, len);
+	if (status != 0)
+		return status;
+
+	write_count = 0;
+	sram_partition = CAD_QSPI_SRAMPART_ADDR(mmio_read_32(CAD_QSPI_OFFSET +
+			 CAD_QSPI_SRAMPART));
+	write_capacity = (uint32_t) CAD_QSPI_SRAM_FIFO_ENTRY_COUNT -
+			sram_partition;
+
+	while (write_count < len) {
+		write_fill_level = CAD_QSPI_SRAMFILL_INDWRPART(
+					mmio_read_32(CAD_QSPI_OFFSET +
+							CAD_QSPI_SRAMFILL));
+		space = LESS(write_capacity - write_fill_level,
+				(len - write_count) / sizeof(uint32_t));
+		write_data = (uint32_t *)(buffer + write_count);
+		for (i = 0; i < space; ++i)
+			mmio_write_32(CAD_QSPIDATA_OFST, *write_data++);
+
+		write_count += space * sizeof(uint32_t);
+	}
+	return cad_qspi_indirect_write_finish();
+}
+
+int cad_qspi_read_bank(uint8_t *buffer, uint32_t offset, uint32_t size)
+{
+	int status;
+	uint32_t read_count = 0, *read_data;
+	int level = 1, count = 0, i;
+
+	status = cad_qspi_indirect_read_start_bank(offset, size);
+
+	if (status != 0)
+		return status;
+
+	while (read_count < size) {
+		do {
+			level = CAD_QSPI_SRAMFILL_INDRDPART(
+				mmio_read_32(CAD_QSPI_OFFSET +
+					CAD_QSPI_SRAMFILL));
+			read_data = (uint32_t *)(buffer + read_count);
+			for (i = 0; i < level; ++i)
+				*read_data++ = mmio_read_32(CAD_QSPIDATA_OFST);
+
+			read_count += level * sizeof(uint32_t);
+			count++;
+		} while (level > 0);
+	}
+
+	return 0;
+}
+
+int cad_qspi_write_bank(uint32_t offset, uint8_t *buffer, uint32_t size)
+{
+	int status = 0;
+	uint32_t page_offset  = offset & (CAD_QSPI_PAGE_SIZE - 1);
+	uint32_t write_size = LESS(size, CAD_QSPI_PAGE_SIZE - page_offset);
+
+	while (size) {
+		status = cad_qspi_indirect_page_bound_write(offset, buffer,
+							write_size);
+		if (status != 0)
+			break;
+
+		offset  += write_size;
+		buffer  += write_size;
+		size -= write_size;
+		write_size = LESS(size, CAD_QSPI_PAGE_SIZE);
+	}
+	return status;
+}
+
+int cad_qspi_read(void *buffer, uint32_t  offset, uint32_t  size)
+{
+	uint32_t bank_count, bank_addr, bank_offset, copy_len;
+	uint8_t *read_data;
+	int i, status;
+
+	status = 0;
+
+	if ((offset >= qspi_device_size) ||
+			(offset + size - 1 >= qspi_device_size) ||
+			(size == 0)) {
+		ERROR("Invalid read parameter\n");
+		return -1;
+	}
+
+	if (CAD_QSPI_INDRD_RD_STAT(mmio_read_32(CAD_QSPI_OFFSET +
+						CAD_QSPI_INDRD))) {
+		ERROR("Read in progress\n");
+		return -1;
+	}
+
+	/*
+	 * bank_count : Number of bank(s) affected, including partial banks.
+	 * bank_addr  : Aligned address of the first bank,
+	 *		including partial bank.
+	 * bank_ofst  : The offset of the bank to read.
+	 *		Only used when reading the first bank.
+	 */
+	bank_count = CAD_QSPI_BANK_ADDR(offset + size - 1) -
+			CAD_QSPI_BANK_ADDR(offset) + 1;
+	bank_addr  = offset & CAD_QSPI_BANK_ADDR_MSK;
+	bank_offset  = offset & (CAD_QSPI_BANK_SIZE - 1);
+
+	read_data = (uint8_t *)buffer;
+
+	copy_len = LESS(size, CAD_QSPI_BANK_SIZE - bank_offset);
+
+	for (i = 0; i < bank_count; ++i) {
+		status = cad_qspi_device_bank_select(CAD_QSPI_BANK_ADDR(
+								bank_addr));
+		if (status != 0)
+			break;
+		status = cad_qspi_read_bank(read_data, bank_offset, copy_len);
+		if (status != 0)
+			break;
+
+		bank_addr += CAD_QSPI_BANK_SIZE;
+		read_data += copy_len;
+		size -= copy_len;
+		bank_offset = 0;
+		copy_len = LESS(size, CAD_QSPI_BANK_SIZE);
+	}
+
+	return status;
+}
+
+int cad_qspi_erase(uint32_t offset, uint32_t size)
+{
+	int status = 0;
+	uint32_t subsector_offset  = offset & (CAD_QSPI_SUBSECTOR_SIZE - 1);
+	uint32_t erase_size = LESS(size,
+				CAD_QSPI_SUBSECTOR_SIZE - subsector_offset);
+
+	while (size) {
+		status = cad_qspi_erase_subsector(offset);
+		if (status != 0)
+			break;
+
+		offset  += erase_size;
+		size -= erase_size;
+		erase_size = LESS(size, CAD_QSPI_SUBSECTOR_SIZE);
+	}
+	return status;
+}
+
+int cad_qspi_write(void *buffer, uint32_t offset, uint32_t size)
+{
+	int status, i;
+	uint32_t bank_count, bank_addr, bank_offset, copy_len;
+	uint8_t *write_data;
+
+	status = 0;
+
+	if ((offset >= qspi_device_size) ||
+			(offset + size - 1 >= qspi_device_size) ||
+			(size == 0)) {
+		return -2;
+	}
+
+	if (CAD_QSPI_INDWR_RDSTAT(mmio_read_32(CAD_QSPI_OFFSET +
+						CAD_QSPI_INDWR))) {
+		ERROR("QSPI Error: Write in progress\n");
+		return -1;
+	}
+
+	bank_count = CAD_QSPI_BANK_ADDR(offset + size - 1) -
+			CAD_QSPI_BANK_ADDR(offset) + 1;
+	bank_addr = offset & CAD_QSPI_BANK_ADDR_MSK;
+	bank_offset = offset & (CAD_QSPI_BANK_SIZE - 1);
+
+	write_data = buffer;
+
+	copy_len = LESS(size, CAD_QSPI_BANK_SIZE - bank_offset);
+
+	for (i = 0; i < bank_count; ++i) {
+		status = cad_qspi_device_bank_select(
+				CAD_QSPI_BANK_ADDR(bank_addr));
+		if (status != 0)
+			break;
+
+		status = cad_qspi_write_bank(bank_offset, write_data,
+						copy_len);
+		if (status != 0)
+			break;
+
+		bank_addr += CAD_QSPI_BANK_SIZE;
+		write_data += copy_len;
+		size -= copy_len;
+		bank_offset = 0;
+
+		copy_len = LESS(size, CAD_QSPI_BANK_SIZE);
+	}
+	return status;
+}
+
+int cad_qspi_update(void *Buffer, uint32_t offset, uint32_t size)
+{
+	int status = 0;
+
+	status = cad_qspi_erase(offset, size);
+	if (status != 0)
+		return status;
+
+	return cad_qspi_write(Buffer, offset, size);
+}
+
+void cad_qspi_reset(void)
+{
+	cad_qspi_stig_cmd(CAD_QSPI_STIG_OPCODE_RESET_EN, 0);
+	cad_qspi_stig_cmd(CAD_QSPI_STIG_OPCODE_RESET_MEM, 0);
+}
+
diff --git a/plat/intel/soc/common/drivers/qspi/cadence_qspi.h b/plat/intel/soc/common/drivers/qspi/cadence_qspi.h
new file mode 100644
index 0000000..104ab5f
--- /dev/null
+++ b/plat/intel/soc/common/drivers/qspi/cadence_qspi.h
@@ -0,0 +1,174 @@
+/*
+ * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CAD_QSPI_H
+#define CAD_QSPI_H
+
+#define CAD_QSPI_MICRON_N25Q_SUPPORT		1
+
+#define CAD_INVALID				-1
+#define CAD_QSPI_ERROR				-2
+
+#define CAD_QSPI_ADDR_FASTREAD			0
+#define CAD_QSPI_ADDR_FASTREAD_DUAL_IO		1
+#define CAD_QSPI_ADDR_FASTREAD_QUAD_IO		2
+#define CAT_QSPI_ADDR_SINGLE_IO			0
+#define CAT_QSPI_ADDR_DUAL_IO			1
+#define CAT_QSPI_ADDR_QUAD_IO			2
+
+#define CAD_QSPI_BANK_ADDR(x)			((x) >> 24)
+#define CAD_QSPI_BANK_ADDR_MSK			0xff000000
+
+#define CAD_QSPI_COMMAND_TIMEOUT		0x10000000
+
+#define CAD_QSPI_CFG				0x0
+#define CAD_QSPI_CFG_BAUDDIV_MSK		0xff87ffff
+#define CAD_QSPI_CFG_BAUDDIV(x)			(((x) << 19) & 0x780000)
+#define CAD_QSPI_CFG_CS_MSK			~0x3c00
+#define CAD_QSPI_CFG_CS(x)			(((x) << 11))
+#define CAD_QSPI_CFG_ENABLE			(1 << 0)
+#define CAD_QSPI_CFG_ENDMA_CLR_MSK		0xffff7fff
+#define CAD_QSPI_CFG_IDLE			(1U << 31)
+#define CAD_QSPI_CFG_SELCLKPHASE_CLR_MSK	0xfffffffb
+#define CAD_QSPI_CFG_SELCLKPOL_CLR_MSK		0xfffffffd
+
+#define CAD_QSPI_DELAY				0xc
+#define CAD_QSPI_DELAY_CSSOT(x)			(((x) & 0xff) << 0)
+#define CAD_QSPI_DELAY_CSEOT(x)			(((x) & 0xff) << 8)
+#define CAD_QSPI_DELAY_CSDADS(x)		(((x) & 0xff) << 16)
+#define CAD_QSPI_DELAY_CSDA(x)			(((x) & 0xff) << 24)
+
+#define CAD_QSPI_DEVSZ				0x14
+#define CAD_QSPI_DEVSZ_ADDR_BYTES(x)		((x) << 0)
+#define CAD_QSPI_DEVSZ_BYTES_PER_PAGE(x)	((x) << 4)
+#define CAD_QSPI_DEVSZ_BYTES_PER_BLOCK(x)	((x) << 16)
+
+#define CAD_QSPI_DEVWR				0x8
+#define CAD_QSPI_DEVRD				0x4
+#define CAD_QSPI_DEV_OPCODE(x)			(((x) & 0xff) << 0)
+#define CAD_QSPI_DEV_INST_TYPE(x)		(((x) & 0x03) << 8)
+#define CAD_QSPI_DEV_ADDR_TYPE(x)		(((x) & 0x03) << 12)
+#define CAD_QSPI_DEV_DATA_TYPE(x)		(((x) & 0x03) << 16)
+#define CAD_QSPI_DEV_MODE_BIT(x)		(((x) & 0x01) << 20)
+#define CAD_QSPI_DEV_DUMMY_CLK_CYCLE(x)		(((x) & 0x0f) << 24)
+
+#define CAD_QSPI_FLASHCMD			0x90
+#define CAD_QSPI_FLASHCMD_ADDR			0x94
+#define CAD_QSPI_FLASHCMD_EXECUTE		0x1
+#define CAD_QSPI_FLASHCMD_EXECUTE_STAT		0x2
+#define CAD_QSPI_FLASHCMD_NUM_DUMMYBYTES_MAX	5
+#define CAD_QSPI_FLASHCMD_NUM_DUMMYBYTES(x)	(((x) << 7) & 0x000f80)
+#define CAD_QSPI_FLASHCMD_OPCODE(x)		(((x) & 0xff) << 24)
+#define CAD_QSPI_FLASHCMD_ENRDDATA(x)		(((x) & 1) << 23)
+#define CAD_QSPI_FLASHCMD_NUMRDDATABYTES(x)	(((x) & 0xf) << 20)
+#define CAD_QSPI_FLASHCMD_ENCMDADDR(x)		(((x) & 1) << 19)
+#define CAD_QSPI_FLASHCMD_ENMODEBIT(x)		(((x) & 1) << 18)
+#define CAD_QSPI_FLASHCMD_NUMADDRBYTES(x)	(((x) & 0x3) << 16)
+#define CAD_QSPI_FLASHCMD_ENWRDATA(x)		(((x) & 1) << 15)
+#define CAD_QSPI_FLASHCMD_NUMWRDATABYTES(x)	(((x) & 0x7) << 12)
+#define CAD_QSPI_FLASHCMD_NUMDUMMYBYTES(x)	(((x) & 0x1f) << 7)
+#define CAD_QSPI_FLASHCMD_RDDATA0		0xa0
+#define CAD_QSPI_FLASHCMD_RDDATA1		0xa4
+#define CAD_QSPI_FLASHCMD_WRDATA0		0xa8
+#define CAD_QSPI_FLASHCMD_WRDATA1		0xac
+
+#define CAD_QSPI_RDDATACAP			0x10
+#define CAD_QSPI_RDDATACAP_BYP(x)		(((x) & 1) << 0)
+#define CAD_QSPI_RDDATACAP_DELAY(x)		(((x) & 0xf) << 1)
+
+#define CAD_QSPI_REMAPADDR			0x24
+#define CAD_QSPI_REMAPADDR_VALUE_SET(x)		(((x) & 0xffffffff) << 0)
+
+#define CAD_QSPI_SRAMPART			0x18
+#define CAD_QSPI_SRAMFILL			0x2c
+#define CAD_QSPI_SRAMPART_ADDR(x)		(((x) >> 0) & 0x3ff)
+#define CAD_QSPI_SRAM_FIFO_ENTRY_COUNT		(512 / sizeof(uint32_t))
+#define CAD_QSPI_SRAMFILL_INDWRPART(x)		(((x) >> 16) & 0x00ffff)
+#define CAD_QSPI_SRAMFILL_INDRDPART(x)		(((x) >> 0) & 0x00ffff)
+
+#define CAD_QSPI_SELCLKPHASE(x)			(((x) & 1) << 2)
+#define CAD_QSPI_SELCLKPOL(x)			(((x) & 1) << 1)
+
+#define CAD_QSPI_STIG_FLAGSR_PROGRAMREADY(x)	(((x) >> 7) & 1)
+#define CAD_QSPI_STIG_FLAGSR_ERASEREADY(x)	(((x) >> 7) & 1)
+#define CAD_QSPI_STIG_FLAGSR_ERASEERROR(x)	(((x) >> 5) & 1)
+#define CAD_QSPI_STIG_FLAGSR_PROGRAMERROR(x)	(((x) >> 4) & 1)
+#define CAD_QSPI_STIG_OPCODE_CLFSR		0x50
+#define CAD_QSPI_STIG_OPCODE_RDID		0x9f
+#define CAD_QSPI_STIG_OPCODE_WRDIS		0x4
+#define CAD_QSPI_STIG_OPCODE_WREN		0x6
+#define CAD_QSPI_STIG_OPCODE_SUBSEC_ERASE	0x20
+#define CAD_QSPI_STIG_OPCODE_SEC_ERASE		0xd8
+#define CAD_QSPI_STIG_OPCODE_WREN_EXT_REG	0xc5
+#define CAD_QSPI_STIG_OPCODE_DIE_ERASE		0xc4
+#define CAD_QSPI_STIG_OPCODE_BULK_ERASE		0xc7
+#define CAD_QSPI_STIG_OPCODE_RDSR		0x5
+#define CAD_QSPI_STIG_OPCODE_RDFLGSR		0x70
+#define CAD_QSPI_STIG_OPCODE_RESET_EN		0x66
+#define CAD_QSPI_STIG_OPCODE_RESET_MEM		0x99
+#define CAD_QSPI_STIG_RDID_CAPACITYID(x)	(((x) >> 16) & 0xff)
+#define CAD_QSPI_STIG_SR_BUSY(x)		(((x) >> 0) & 1)
+
+
+#define CAD_QSPI_INST_SINGLE			0
+#define CAD_QSPI_INST_DUAL			1
+#define CAD_QSPI_INST_QUAD			2
+
+#define CAD_QSPI_INDRDSTADDR			0x68
+#define CAD_QSPI_INDRDCNT			0x6c
+#define CAD_QSPI_INDRD				0x60
+#define CAD_QSPI_INDRD_RD_STAT(x)		(((x) >> 2) & 1)
+#define CAD_QSPI_INDRD_START			1
+#define CAD_QSPI_INDRD_IND_OPS_DONE		0x20
+
+#define CAD_QSPI_INDWR				0x70
+#define CAD_QSPI_INDWR_RDSTAT(x)		(((x) >> 2) & 1)
+#define CAD_QSPI_INDWRSTADDR			0x78
+#define CAD_QSPI_INDWRCNT			0x7c
+#define CAD_QSPI_INDWR				0x70
+#define CAD_QSPI_INDWR_START			0x1
+#define CAD_QSPI_INDWR_INDDONE			0x20
+
+#define CAD_QSPI_INT_STATUS_ALL			0x0000ffff
+
+#define CAD_QSPI_N25Q_DIE_SIZE			0x02000000
+#define CAD_QSPI_BANK_SIZE			0x01000000
+#define CAD_QSPI_PAGE_SIZE			0x00000100
+
+#define CAD_QSPI_IRQMSK				0x44
+
+#define CAD_QSPI_SUBSECTOR_SIZE			0x1000
+
+#define INTEL_QSPI_ADDR_BYTES			2
+#define INTEL_QSPI_BYTES_PER_DEV		256
+#define INTEL_BYTES_PER_BLOCK			16
+
+#define QSPI_FAST_READ				0xb
+
+#define QSPI_WRITE				0x2
+
+// QSPI CONFIGURATIONS
+
+#define QSPI_CONFIG_CPOL			1
+#define QSPI_CONFIG_CPHA			1
+
+#define QSPI_CONFIG_CSSOT			0x14
+#define QSPI_CONFIG_CSEOT			0x14
+#define QSPI_CONFIG_CSDADS			0xff
+#define QSPI_CONFIG_CSDA			0xc8
+
+int cad_qspi_init(uint32_t desired_clk_freq, uint32_t clk_phase,
+	uint32_t clk_pol, uint32_t csda, uint32_t csdads,
+	uint32_t cseot, uint32_t cssot, uint32_t rddatacap);
+void cad_qspi_set_chip_select(int cs);
+int cad_qspi_erase(uint32_t offset, uint32_t size);
+int cad_qspi_write(void *buffer, uint32_t offset, uint32_t size);
+int cad_qspi_read(void *buffer, uint32_t offset, uint32_t size);
+int cad_qspi_update(void *buffer, uint32_t offset, uint32_t size);
+
+#endif
+
diff --git a/plat/intel/soc/common/drivers/sdmmc/sdmmc.c b/plat/intel/soc/common/drivers/sdmmc/sdmmc.c
new file mode 100644
index 0000000..8666f54
--- /dev/null
+++ b/plat/intel/soc/common/drivers/sdmmc/sdmmc.c
@@ -0,0 +1,769 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/cadence/cdns_combo_phy.h>
+#include <drivers/cadence/cdns_sdmmc.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+
+#include "agilex5_pinmux.h"
+#include "sdmmc.h"
+
+static const struct mmc_ops *ops;
+static unsigned int mmc_ocr_value;
+static struct mmc_csd_emmc mmc_csd;
+static struct sd_switch_status sd_switch_func_status;
+static unsigned char mmc_ext_csd[512] __aligned(16);
+static unsigned int mmc_flags;
+static struct mmc_device_info *mmc_dev_info;
+static unsigned int rca;
+static unsigned int scr[2]__aligned(16) = { 0 };
+
+extern const struct mmc_ops cdns_sdmmc_ops;
+extern struct cdns_sdmmc_params cdns_params;
+extern struct cdns_sdmmc_combo_phy sdmmc_combo_phy_reg;
+extern struct cdns_sdmmc_sdhc sdmmc_sdhc_reg;
+
+static bool is_cmd23_enabled(void)
+{
+	return ((mmc_flags & MMC_FLAG_CMD23) != 0U);
+}
+
+static bool is_sd_cmd6_enabled(void)
+{
+	return ((mmc_flags & MMC_FLAG_SD_CMD6) != 0U);
+}
+
+/* TODO: Will romove once ATF driver is developed */
+void sdmmc_pin_config(void)
+{
+	/* temp use base + addr. Official must change to common method */
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x00, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x04, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x08, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x0C, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x10, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x14, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x18, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x1C, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x20, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x24, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x28, 0x0);
+}
+
+static int sdmmc_send_cmd(unsigned int idx, unsigned int arg,
+			unsigned int r_type, unsigned int *r_data)
+{
+	struct mmc_cmd cmd;
+	int ret;
+
+	zeromem(&cmd, sizeof(struct mmc_cmd));
+
+	cmd.cmd_idx = idx;
+	cmd.cmd_arg = arg;
+	cmd.resp_type = r_type;
+
+	ret = ops->send_cmd(&cmd);
+
+	if ((ret == 0) && (r_data != NULL)) {
+		int i;
+
+		for (i = 0; i < 4; i++) {
+			*r_data = cmd.resp_data[i];
+			r_data++;
+		}
+	}
+
+	if (ret != 0) {
+		VERBOSE("Send command %u error: %d\n", idx, ret);
+	}
+
+	return ret;
+}
+
+static int sdmmc_device_state(void)
+{
+	int retries = DEFAULT_SDMMC_MAX_RETRIES;
+	unsigned int resp_data[4];
+
+	do {
+		int ret;
+
+		if (retries == 0) {
+			ERROR("CMD13 failed after %d retries\n",
+			      DEFAULT_SDMMC_MAX_RETRIES);
+			return -EIO;
+		}
+
+		ret = sdmmc_send_cmd(MMC_CMD(13), rca << RCA_SHIFT_OFFSET,
+				   MMC_RESPONSE_R1, &resp_data[0]);
+		if (ret != 0) {
+			retries--;
+			continue;
+		}
+
+		if ((resp_data[0] & STATUS_SWITCH_ERROR) != 0U) {
+			return -EIO;
+		}
+
+		retries--;
+	} while ((resp_data[0] & STATUS_READY_FOR_DATA) == 0U);
+
+	return MMC_GET_STATE(resp_data[0]);
+}
+
+static int sdmmc_set_ext_csd(unsigned int ext_cmd, unsigned int value)
+{
+	int ret;
+
+	ret = sdmmc_send_cmd(MMC_CMD(6),
+			   EXTCSD_WRITE_BYTES | EXTCSD_CMD(ext_cmd) |
+			   EXTCSD_VALUE(value) | EXTCSD_CMD_SET_NORMAL,
+			   MMC_RESPONSE_R1B, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	do {
+		ret = sdmmc_device_state();
+		if (ret < 0) {
+			return ret;
+		}
+	} while (ret == MMC_STATE_PRG);
+
+	return 0;
+}
+
+static int sdmmc_mmc_sd_switch(unsigned int bus_width)
+{
+	int ret;
+	int retries = DEFAULT_SDMMC_MAX_RETRIES;
+	unsigned int bus_width_arg = 0;
+
+	/* CMD55: Application Specific Command */
+	ret = sdmmc_send_cmd(MMC_CMD(55), rca << RCA_SHIFT_OFFSET,
+			   MMC_RESPONSE_R5, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = ops->prepare(0, (uintptr_t)&scr, sizeof(scr));
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* ACMD51: SEND_SCR */
+	do {
+		ret = sdmmc_send_cmd(MMC_ACMD(51), 0, MMC_RESPONSE_R1, NULL);
+		if ((ret != 0) && (retries == 0)) {
+			ERROR("ACMD51 failed after %d retries (ret=%d)\n",
+			      DEFAULT_SDMMC_MAX_RETRIES, ret);
+			return ret;
+		}
+
+		retries--;
+	} while (ret != 0);
+
+	ret = ops->read(0, (uintptr_t)&scr, sizeof(scr));
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (((scr[0] & SD_SCR_BUS_WIDTH_4) != 0U) &&
+	    (bus_width == MMC_BUS_WIDTH_4)) {
+		bus_width_arg = 2;
+	}
+
+	/* CMD55: Application Specific Command */
+	ret = sdmmc_send_cmd(MMC_CMD(55), rca << RCA_SHIFT_OFFSET,
+			   MMC_RESPONSE_R5, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* ACMD6: SET_BUS_WIDTH */
+	ret = sdmmc_send_cmd(MMC_ACMD(6), bus_width_arg, MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	do {
+		ret = sdmmc_device_state();
+		if (ret < 0) {
+			return ret;
+		}
+	} while (ret == MMC_STATE_PRG);
+
+	return 0;
+}
+
+static int sdmmc_set_ios(unsigned int clk, unsigned int bus_width)
+{
+	int ret;
+	unsigned int width = bus_width;
+
+	if (mmc_dev_info->mmc_dev_type != MMC_IS_EMMC) {
+		if (width == MMC_BUS_WIDTH_8) {
+			WARN("Wrong bus config for SD-card, force to 4\n");
+			width = MMC_BUS_WIDTH_4;
+		}
+		ret = sdmmc_mmc_sd_switch(width);
+		if (ret != 0) {
+			return ret;
+		}
+	} else if (mmc_csd.spec_vers == 4U) {
+		ret = sdmmc_set_ext_csd(CMD_EXTCSD_BUS_WIDTH,
+				      (unsigned int)width);
+		if (ret != 0) {
+			return ret;
+		}
+	} else {
+		VERBOSE("Wrong MMC type or spec version\n");
+	}
+
+	return ops->set_ios(clk, width);
+}
+
+static int sdmmc_fill_device_info(void)
+{
+	unsigned long long c_size;
+	unsigned int speed_idx;
+	unsigned int nb_blocks;
+	unsigned int freq_unit;
+	int ret = 0;
+	struct mmc_csd_sd_v2 *csd_sd_v2;
+
+	switch (mmc_dev_info->mmc_dev_type) {
+	case MMC_IS_EMMC:
+		mmc_dev_info->block_size = MMC_BLOCK_SIZE;
+
+		ret = ops->prepare(0, (uintptr_t)&mmc_ext_csd,
+				   sizeof(mmc_ext_csd));
+		if (ret != 0) {
+			return ret;
+		}
+
+		/* MMC CMD8: SEND_EXT_CSD */
+		ret = sdmmc_send_cmd(MMC_CMD(8), 0, MMC_RESPONSE_R1, NULL);
+		if (ret != 0) {
+			return ret;
+		}
+
+		ret = ops->read(0, (uintptr_t)&mmc_ext_csd,
+				sizeof(mmc_ext_csd));
+		if (ret != 0) {
+			return ret;
+		}
+
+		do {
+			ret = sdmmc_device_state();
+			if (ret < 0) {
+				return ret;
+			}
+		} while (ret != MMC_STATE_TRAN);
+
+		nb_blocks = (mmc_ext_csd[CMD_EXTCSD_SEC_CNT] << 0) |
+			    (mmc_ext_csd[CMD_EXTCSD_SEC_CNT + 1] << 8) |
+			    (mmc_ext_csd[CMD_EXTCSD_SEC_CNT + 2] << 16) |
+			    (mmc_ext_csd[CMD_EXTCSD_SEC_CNT + 3] << 24);
+
+		mmc_dev_info->device_size = (unsigned long long)nb_blocks *
+			mmc_dev_info->block_size;
+
+		break;
+
+	case MMC_IS_SD:
+		/*
+		 * Use the same mmc_csd struct, as required fields here
+		 * (READ_BL_LEN, C_SIZE, CSIZE_MULT) are common with eMMC.
+		 */
+		mmc_dev_info->block_size = BIT_32(mmc_csd.read_bl_len);
+
+		c_size = ((unsigned long long)mmc_csd.c_size_high << 2U) |
+			 (unsigned long long)mmc_csd.c_size_low;
+		assert(c_size != 0xFFFU);
+
+		mmc_dev_info->device_size = (c_size + 1U) *
+					    BIT_64(mmc_csd.c_size_mult + 2U) *
+					    mmc_dev_info->block_size;
+
+		break;
+
+	case MMC_IS_SD_HC:
+		assert(mmc_csd.csd_structure == 1U);
+
+		mmc_dev_info->block_size = MMC_BLOCK_SIZE;
+
+		/* Need to use mmc_csd_sd_v2 struct */
+		csd_sd_v2 = (struct mmc_csd_sd_v2 *)&mmc_csd;
+		c_size = ((unsigned long long)csd_sd_v2->c_size_high << 16) |
+			 (unsigned long long)csd_sd_v2->c_size_low;
+
+		mmc_dev_info->device_size = (c_size + 1U) << SDMMC_MULT_BY_512K_SHIFT;
+
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	if (ret < 0) {
+		return ret;
+	}
+
+	speed_idx = (mmc_csd.tran_speed & CSD_TRAN_SPEED_MULT_MASK) >>
+			 CSD_TRAN_SPEED_MULT_SHIFT;
+
+	assert(speed_idx > 0U);
+
+	if (mmc_dev_info->mmc_dev_type == MMC_IS_EMMC) {
+		mmc_dev_info->max_bus_freq = tran_speed_base[speed_idx];
+	} else {
+		mmc_dev_info->max_bus_freq = sd_tran_speed_base[speed_idx];
+	}
+
+	freq_unit = mmc_csd.tran_speed & CSD_TRAN_SPEED_UNIT_MASK;
+	while (freq_unit != 0U) {
+		mmc_dev_info->max_bus_freq *= 10U;
+		--freq_unit;
+	}
+
+	mmc_dev_info->max_bus_freq *= 10000U;
+
+	return 0;
+}
+
+static int sdmmc_sd_switch(unsigned int mode, unsigned char group,
+		     unsigned char func)
+{
+	unsigned int group_shift = (group - 1U) * 4U;
+	unsigned int group_mask = GENMASK(group_shift + 3U,  group_shift);
+	unsigned int arg;
+	int ret;
+
+	ret = ops->prepare(0, (uintptr_t)&sd_switch_func_status,
+			   sizeof(sd_switch_func_status));
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* MMC CMD6: SWITCH_FUNC */
+	arg = mode | SD_SWITCH_ALL_GROUPS_MASK;
+	arg &= ~group_mask;
+	arg |= func << group_shift;
+	ret = sdmmc_send_cmd(MMC_CMD(6), arg, MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	return ops->read(0, (uintptr_t)&sd_switch_func_status,
+			 sizeof(sd_switch_func_status));
+}
+
+static int sdmmc_sd_send_op_cond(void)
+{
+	int n;
+	unsigned int resp_data[4];
+
+	for (n = 0; n < SEND_SDMMC_OP_COND_MAX_RETRIES; n++) {
+		int ret;
+
+		/* CMD55: Application Specific Command */
+		ret = sdmmc_send_cmd(MMC_CMD(55), 0, MMC_RESPONSE_R1, NULL);
+		if (ret != 0) {
+			return ret;
+		}
+
+		/* ACMD41: SD_SEND_OP_COND */
+		ret = sdmmc_send_cmd(MMC_ACMD(41), OCR_HCS |
+			mmc_dev_info->ocr_voltage, MMC_RESPONSE_R3,
+			&resp_data[0]);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((resp_data[0] & OCR_POWERUP) != 0U) {
+			mmc_ocr_value = resp_data[0];
+
+			if ((mmc_ocr_value & OCR_HCS) != 0U) {
+				mmc_dev_info->mmc_dev_type = MMC_IS_SD_HC;
+			} else {
+				mmc_dev_info->mmc_dev_type = MMC_IS_SD;
+			}
+
+			return 0;
+		}
+
+		mdelay(10);
+	}
+
+	ERROR("ACMD41 failed after %d retries\n", SEND_SDMMC_OP_COND_MAX_RETRIES);
+
+	return -EIO;
+}
+
+static int sdmmc_reset_to_idle(void)
+{
+	int ret;
+
+	/* CMD0: reset to IDLE */
+	ret = sdmmc_send_cmd(MMC_CMD(0), 0, 0, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	mdelay(2);
+
+	return 0;
+}
+
+static int sdmmc_send_op_cond(void)
+{
+	int ret, n;
+	unsigned int resp_data[4];
+
+	ret = sdmmc_reset_to_idle();
+	if (ret != 0) {
+		return ret;
+	}
+
+	for (n = 0; n < SEND_SDMMC_OP_COND_MAX_RETRIES; n++) {
+		ret = sdmmc_send_cmd(MMC_CMD(1), OCR_SECTOR_MODE |
+				   OCR_VDD_MIN_2V7 | OCR_VDD_MIN_1V7,
+				   MMC_RESPONSE_R3, &resp_data[0]);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((resp_data[0] & OCR_POWERUP) != 0U) {
+			mmc_ocr_value = resp_data[0];
+			return 0;
+		}
+
+		mdelay(10);
+	}
+
+	ERROR("CMD1 failed after %d retries\n", SEND_SDMMC_OP_COND_MAX_RETRIES);
+
+	return -EIO;
+}
+
+static int sdmmc_enumerate(unsigned int clk, unsigned int bus_width)
+{
+	int ret;
+	unsigned int resp_data[4];
+
+	ops->init();
+
+	ret = sdmmc_reset_to_idle();
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (mmc_dev_info->mmc_dev_type == MMC_IS_EMMC) {
+		ret = sdmmc_send_op_cond();
+	} else {
+		/* CMD8: Send Interface Condition Command */
+		ret = sdmmc_send_cmd(MMC_CMD(8), VHS_2_7_3_6_V | CMD8_CHECK_PATTERN,
+				   MMC_RESPONSE_R5, &resp_data[0]);
+
+		if ((ret == 0) && ((resp_data[0] & 0xffU) == CMD8_CHECK_PATTERN)) {
+			ret = sdmmc_sd_send_op_cond();
+		}
+	}
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* CMD2: Card Identification */
+	ret = sdmmc_send_cmd(MMC_CMD(2), 0, MMC_RESPONSE_R2, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* CMD3: Set Relative Address */
+	if (mmc_dev_info->mmc_dev_type == MMC_IS_EMMC) {
+		rca = MMC_FIX_RCA;
+		ret = sdmmc_send_cmd(MMC_CMD(3), rca << RCA_SHIFT_OFFSET,
+				   MMC_RESPONSE_R1, NULL);
+		if (ret != 0) {
+			return ret;
+		}
+	} else {
+		ret = sdmmc_send_cmd(MMC_CMD(3), 0,
+				   MMC_RESPONSE_R6, &resp_data[0]);
+		if (ret != 0) {
+			return ret;
+		}
+
+		rca = (resp_data[0] & 0xFFFF0000U) >> 16;
+	}
+
+	/* CMD9: CSD Register */
+	ret = sdmmc_send_cmd(MMC_CMD(9), rca << RCA_SHIFT_OFFSET,
+			   MMC_RESPONSE_R2, &resp_data[0]);
+	if (ret != 0) {
+		return ret;
+	}
+
+	memcpy(&mmc_csd, &resp_data, sizeof(resp_data));
+
+	/* CMD7: Select Card */
+	ret = sdmmc_send_cmd(MMC_CMD(7), rca << RCA_SHIFT_OFFSET,
+			   MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	do {
+		ret = sdmmc_device_state();
+		if (ret < 0) {
+			return ret;
+		}
+	} while (ret != MMC_STATE_TRAN);
+
+	ret = sdmmc_set_ios(clk, bus_width);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = sdmmc_fill_device_info();
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (is_sd_cmd6_enabled() &&
+	    (mmc_dev_info->mmc_dev_type == MMC_IS_SD_HC)) {
+		/* Try to switch to High Speed Mode */
+		ret = sdmmc_sd_switch(SD_SWITCH_FUNC_CHECK, 1U, 1U);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((sd_switch_func_status.support_g1 & BIT(9)) == 0U) {
+			/* High speed not supported, keep default speed */
+			return 0;
+		}
+
+		ret = sdmmc_sd_switch(SD_SWITCH_FUNC_SWITCH, 1U, 1U);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((sd_switch_func_status.sel_g2_g1 & 0x1U) == 0U) {
+			/* Cannot switch to high speed, keep default speed */
+			return 0;
+		}
+
+		mmc_dev_info->max_bus_freq = 50000000U;
+		ret = ops->set_ios(clk, bus_width);
+	}
+
+	return ret;
+}
+
+size_t sdmmc_read_blocks(int lba, uintptr_t buf, size_t size)
+{
+	int ret;
+	unsigned int cmd_idx, cmd_arg;
+
+	assert((ops != NULL) &&
+	       (ops->read != NULL) &&
+	       (size != 0U) &&
+	       ((size & MMC_BLOCK_MASK) == 0U));
+
+	ret = ops->prepare(lba, buf, size);
+	if (ret != 0) {
+		return 0;
+	}
+
+	if (is_cmd23_enabled()) {
+		/* Set block count */
+		ret = sdmmc_send_cmd(MMC_CMD(23), size / MMC_BLOCK_SIZE,
+				   MMC_RESPONSE_R1, NULL);
+		if (ret != 0) {
+			return 0;
+		}
+
+		cmd_idx = MMC_CMD(18);
+	} else {
+		if (size > MMC_BLOCK_SIZE) {
+			cmd_idx = MMC_CMD(18);
+		} else {
+			cmd_idx = MMC_CMD(17);
+		}
+	}
+
+	if (((mmc_ocr_value & OCR_ACCESS_MODE_MASK) == OCR_BYTE_MODE) &&
+	    (mmc_dev_info->mmc_dev_type != MMC_IS_SD_HC)) {
+		cmd_arg = lba * MMC_BLOCK_SIZE;
+	} else {
+		cmd_arg = lba;
+	}
+
+	ret = sdmmc_send_cmd(cmd_idx, cmd_arg, MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return 0;
+	}
+
+	ret = ops->read(lba, buf, size);
+	if (ret != 0) {
+		return 0;
+	}
+
+	/* Wait buffer empty */
+	do {
+		ret = sdmmc_device_state();
+		if (ret < 0) {
+			return 0;
+		}
+	} while ((ret != MMC_STATE_TRAN) && (ret != MMC_STATE_DATA));
+
+	if (!is_cmd23_enabled() && (size > MMC_BLOCK_SIZE)) {
+		ret = sdmmc_send_cmd(MMC_CMD(12), 0, MMC_RESPONSE_R1B, NULL);
+		if (ret != 0) {
+			return 0;
+		}
+	}
+
+	return size;
+}
+
+size_t sdmmc_write_blocks(int lba, const uintptr_t buf, size_t size)
+{
+	int ret;
+	unsigned int cmd_idx, cmd_arg;
+
+	assert((ops != NULL) &&
+	       (ops->write != NULL) &&
+	       (size != 0U) &&
+	       ((buf & MMC_BLOCK_MASK) == 0U) &&
+	       ((size & MMC_BLOCK_MASK) == 0U));
+
+	ret = ops->prepare(lba, buf, size);
+	if (ret != 0) {
+		return 0;
+	}
+
+	if (is_cmd23_enabled()) {
+		/* Set block count */
+		ret = sdmmc_send_cmd(MMC_CMD(23), size / MMC_BLOCK_SIZE,
+				   MMC_RESPONSE_R1, NULL);
+		if (ret != 0) {
+			return 0;
+		}
+
+		cmd_idx = MMC_CMD(25);
+	} else {
+		if (size > MMC_BLOCK_SIZE) {
+			cmd_idx = MMC_CMD(25);
+		} else {
+			cmd_idx = MMC_CMD(24);
+		}
+	}
+
+	if ((mmc_ocr_value & OCR_ACCESS_MODE_MASK) == OCR_BYTE_MODE) {
+		cmd_arg = lba * MMC_BLOCK_SIZE;
+	} else {
+		cmd_arg = lba;
+	}
+
+	ret = sdmmc_send_cmd(cmd_idx, cmd_arg, MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return 0;
+	}
+
+	ret = ops->write(lba, buf, size);
+	if (ret != 0) {
+		return 0;
+	}
+
+	/* Wait buffer empty */
+	do {
+		ret = sdmmc_device_state();
+		if (ret < 0) {
+			return 0;
+		}
+	} while ((ret != MMC_STATE_TRAN) && (ret != MMC_STATE_RCV));
+
+	if (!is_cmd23_enabled() && (size > MMC_BLOCK_SIZE)) {
+		ret = sdmmc_send_cmd(MMC_CMD(12), 0, MMC_RESPONSE_R1B, NULL);
+		if (ret != 0) {
+			return 0;
+		}
+	}
+
+	return size;
+}
+
+int sd_or_mmc_init(const struct mmc_ops *ops_ptr, unsigned int clk,
+	     unsigned int width, unsigned int flags,
+	     struct mmc_device_info *device_info)
+{
+	assert((ops_ptr != NULL) &&
+	       (ops_ptr->init != NULL) &&
+	       (ops_ptr->send_cmd != NULL) &&
+	       (ops_ptr->set_ios != NULL) &&
+	       (ops_ptr->prepare != NULL) &&
+	       (ops_ptr->read != NULL) &&
+	       (ops_ptr->write != NULL) &&
+	       (device_info != NULL) &&
+	       (clk != 0) &&
+	       ((width == MMC_BUS_WIDTH_1) ||
+		(width == MMC_BUS_WIDTH_4) ||
+		(width == MMC_BUS_WIDTH_8) ||
+		(width == MMC_BUS_WIDTH_DDR_4) ||
+		(width == MMC_BUS_WIDTH_DDR_8)));
+
+	ops = ops_ptr;
+	mmc_flags = flags;
+	mmc_dev_info = device_info;
+
+	return sdmmc_enumerate(clk, width);
+}
+
+int sdmmc_init(handoff *hoff_ptr, struct cdns_sdmmc_params *params, struct mmc_device_info *info)
+{
+	int result = 0;
+
+	/* SDMMC pin mux configuration */
+	sdmmc_pin_config();
+	cdns_set_sdmmc_var(&sdmmc_combo_phy_reg, &sdmmc_sdhc_reg);
+	result = cdns_sd_host_init(&sdmmc_combo_phy_reg, &sdmmc_sdhc_reg);
+	if (result < 0) {
+		return result;
+	}
+
+	assert((params != NULL) &&
+	       ((params->reg_base & MMC_BLOCK_MASK) == 0) &&
+	       ((params->desc_base & MMC_BLOCK_MASK) == 0) &&
+	       ((params->desc_size & MMC_BLOCK_MASK) == 0) &&
+		   ((params->reg_pinmux & MMC_BLOCK_MASK) == 0) &&
+		   ((params->reg_phy & MMC_BLOCK_MASK) == 0) &&
+	       (params->desc_size > 0) &&
+	       (params->clk_rate > 0) &&
+	       ((params->bus_width == MMC_BUS_WIDTH_1) ||
+		(params->bus_width == MMC_BUS_WIDTH_4) ||
+		(params->bus_width == MMC_BUS_WIDTH_8)));
+
+	memcpy(&cdns_params, params, sizeof(struct cdns_sdmmc_params));
+	cdns_params.cdn_sdmmc_dev_type = info->mmc_dev_type;
+	cdns_params.cdn_sdmmc_dev_mode = SD_DS;
+
+	result = sd_or_mmc_init(&cdns_sdmmc_ops, params->clk_rate, params->bus_width,
+		params->flags, info);
+
+	return result;
+}
diff --git a/plat/intel/soc/common/drivers/sdmmc/sdmmc.h b/plat/intel/soc/common/drivers/sdmmc/sdmmc.h
new file mode 100644
index 0000000..16c6b04
--- /dev/null
+++ b/plat/intel/soc/common/drivers/sdmmc/sdmmc.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SDMMC_H
+#define SDMMC_H
+
+#include <lib/mmio.h>
+#include "socfpga_handoff.h"
+
+#define PERIPHERAL_SDMMC_MASK			0x60
+#define PERIPHERAL_SDMMC_OFFSET			6
+
+#define DEFAULT_SDMMC_MAX_RETRIES		5
+#define SEND_SDMMC_OP_COND_MAX_RETRIES		100
+#define SDMMC_MULT_BY_512K_SHIFT		19
+
+static const unsigned char tran_speed_base[16] = {
+	0, 10, 12, 13, 15, 20, 26, 30, 35, 40, 45, 52, 55, 60, 70, 80
+};
+
+static const unsigned char sd_tran_speed_base[16] = {
+	0, 10, 12, 13, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 70, 80
+};
+
+
+/* FUNCTION DEFINATION */
+/*
+ * @brief SDMMC controller initialization function
+ *
+ * @hoff_ptr: Pointer to the hand-off data
+ * Return: 0 on success, a negative errno on failure
+ */
+int sdmmc_init(handoff *hoff_ptr, struct cdns_sdmmc_params *params,
+	     struct mmc_device_info *info);
+int sd_or_mmc_init(const struct mmc_ops *ops_ptr, unsigned int clk,
+	     unsigned int width, unsigned int flags,
+	     struct mmc_device_info *device_info);
+void sdmmc_pin_config(void);
+#endif
diff --git a/plat/intel/soc/common/drivers/wdt/watchdog.c b/plat/intel/soc/common/drivers/wdt/watchdog.c
new file mode 100644
index 0000000..651189b
--- /dev/null
+++ b/plat/intel/soc/common/drivers/wdt/watchdog.c
@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include "watchdog.h"
+
+
+/* Reset watchdog timer */
+void watchdog_sw_rst(void)
+{
+	mmio_write_32(WDT_CRR, WDT_SW_RST);
+}
+
+/* Print component information */
+void watchdog_info(void)
+{
+	INFO("Component Type    : %x\r\n", mmio_read_32(WDT_COMP_VERSION));
+	INFO("Component Version : %x\r\n", mmio_read_32(WDT_COMP_TYPE));
+}
+
+/* Check watchdog current status */
+void watchdog_status(void)
+{
+	if (mmio_read_32(WDT_CR) & 1) {
+		INFO("Watchdog Timer is currently enabled\n");
+		INFO("Current Counter : 0x%x\r\n", mmio_read_32(WDT_CCVR));
+	} else {
+		INFO("Watchdog Timer is currently disabled\n");
+	}
+}
+
+/* Initialize & enable watchdog */
+void watchdog_init(int watchdog_clk)
+{
+	uint8_t cycles_i = 0;
+	uint32_t wdt_cycles = WDT_MIN_CYCLES;
+	uint32_t top_init_cycles = WDT_PERIOD * watchdog_clk;
+
+	while ((cycles_i < 15) && (wdt_cycles < top_init_cycles)) {
+		wdt_cycles = (wdt_cycles << 1);
+		cycles_i++;
+	}
+
+	mmio_write_32(WDT_TORR, (cycles_i << 4) | cycles_i);
+
+	mmio_write_32(WDT_CR, WDT_CR_RMOD|WDT_CR_EN);
+}
diff --git a/plat/intel/soc/common/drivers/wdt/watchdog.h b/plat/intel/soc/common/drivers/wdt/watchdog.h
new file mode 100644
index 0000000..4ee4cff
--- /dev/null
+++ b/plat/intel/soc/common/drivers/wdt/watchdog.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CAD_WATCHDOG_H
+#define CAD_WATCHDOG_H
+
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+#define WDT_BASE			(0x10D00200)
+#else
+#define WDT_BASE			(0xFFD00200)
+#endif
+#define WDT_REG_SIZE_OFFSET		(0x4)
+#define WDT_MIN_CYCLES			(65536)
+#define WDT_PERIOD			(20)
+
+#define WDT_CR				(WDT_BASE + 0x0)
+#define WDT_TORR			(WDT_BASE + 0x4)
+
+#define WDT_CRR				(WDT_BASE + 0xC)
+
+#define WDT_CCVR			(WDT_BASE + 0x8)
+#define WDT_STAT			(WDT_BASE + 0x10)
+#define WDT_EOI				(WDT_BASE + 0x14)
+
+#define WDT_COMP_PARAM_1		(WDT_BASE + 0xF4)
+#define WDT_COMP_VERSION		(WDT_BASE + 0xF8)
+#define WDT_COMP_TYPE			(WDT_BASE + 0XFC)
+
+#define WDT_CR_RMOD			(0x0)
+#define WDT_CR_EN			(0x1)
+
+#define WDT_SW_RST			(0x76)
+
+
+void watchdog_init(int watchdog_clk);
+void watchdog_info(void);
+void watchdog_status(void);
+void watchdog_sw_rst(void);
+
+#endif