diff --git a/.cz.json b/.cz.json
index 5ac961e..3c28d3c 100644
--- a/.cz.json
+++ b/.cz.json
@@ -809,6 +809,10 @@
                 {
                     "title": "STM32 Image",
                     "scopes": ["stm32image", "tools/stm32image"]
+                },
+                {
+                    "title": "fiptool",
+                    "scopes": ["fiptool"]
                 }
             ]
         },
diff --git a/Makefile b/Makefile
index 32eb501..ed7b076 100644
--- a/Makefile
+++ b/Makefile
@@ -794,9 +794,7 @@
     endif
     ifeq (${ENABLE_SVE_FOR_NS},1)
         # Warning instead of error due to CI dependency on this
-        $(warning "ENABLE_SVE_FOR_NS cannot be used with ARCH=aarch32")
-        $(warning "Forced ENABLE_SVE_FOR_NS=0")
-        override ENABLE_SVE_FOR_NS	:= 0
+        $(error "ENABLE_SVE_FOR_NS cannot be used with ARCH=aarch32")
     endif
 endif
 
@@ -1443,7 +1441,7 @@
 
 ${FIPTOOL}: FORCE
 ifdef UNIX_MK
-	${Q}${MAKE} CPPFLAGS="-DVERSION='\"${VERSION_STRING}\"'" FIPTOOL=${FIPTOOL} --no-print-directory -C ${FIPTOOLPATH}
+	${Q}${MAKE} CPPFLAGS="-DVERSION='\"${VERSION_STRING}\"'" FIPTOOL=${FIPTOOL} OPENSSL_DIR=${OPENSSL_DIR} --no-print-directory -C ${FIPTOOLPATH}
 else
 # Clear the MAKEFLAGS as we do not want
 # to pass the gnumake flags to nmake.
diff --git a/docs/design/cpu-specific-build-macros.rst b/docs/design/cpu-specific-build-macros.rst
index 9d0dd5e..7075ca6 100644
--- a/docs/design/cpu-specific-build-macros.rst
+++ b/docs/design/cpu-specific-build-macros.rst
@@ -441,6 +441,19 @@
 -  ``ERRATA_N2_2280757``: This applies errata 2280757 workaround to Neoverse-N2
    CPU. This needs to be enabled for revision r0p0 of the CPU and is still open.
 
+For Cortex-X2, the following errata build flags are defined :
+
+-  ``ERRATA_X2_2002765``: This applies errata 2002765 workaround to Cortex-X2
+   CPU. This needs to be enabled for revisions r0p0, r1p0, and r2p0 of the CPU,
+   it is still open.
+
+-  ``ERRATA_X2_2058056``: This applies errata 2058056 workaround to Cortex-X2
+   CPU. This needs to be enabled for revisions r0p0, r1p0, and r2p0 of the CPU,
+   it is still open.
+
+-  ``ERRATA_X2_2083908``: This applies errata 2083908 workaround to Cortex-X2
+   CPU. This needs to be enabled for revision r2p0 of the CPU, it is still open.
+
 DSU Errata Workarounds
 ----------------------
 
diff --git a/drivers/allwinner/axp/axp803.c b/drivers/allwinner/axp/axp803.c
index 53b11c1..7050818 100644
--- a/drivers/allwinner/axp/axp803.c
+++ b/drivers/allwinner/axp/axp803.c
@@ -9,6 +9,7 @@
 const uint8_t axp_chip_id = AXP803_CHIP_ID;
 const char *const axp_compatible = "x-powers,axp803";
 
+#if SUNXI_SETUP_REGULATORS == 1
 const struct axp_regulator axp_regulators[] = {
 	{"dcdc1", 1600, 3400, 100, NA, 0x20, 0x10, 0},
 	{"dcdc5",  800, 1840,  10, 32, 0x24, 0x10, 4},
@@ -20,3 +21,4 @@
 	{"fldo1",  700, 1450,  50, NA, 0x1c, 0x13, 2},
 	{}
 };
+#endif
diff --git a/drivers/allwinner/axp/axp805.c b/drivers/allwinner/axp/axp805.c
index 8d029c0..3a03fec 100644
--- a/drivers/allwinner/axp/axp805.c
+++ b/drivers/allwinner/axp/axp805.c
@@ -9,6 +9,7 @@
 const uint8_t axp_chip_id = AXP805_CHIP_ID;
 const char *const axp_compatible = "x-powers,axp805";
 
+#if SUNXI_SETUP_REGULATORS == 1
 /*
  * The "dcdcd" split changes the step size by a factor of 5, not 2;
  * disallow values above the split to maintain accuracy.
@@ -31,3 +32,4 @@
 	{"cldo3",  700, 3300, 100, NA, 0x26, 0x11, 6},
 	{}
 };
+#endif
diff --git a/drivers/allwinner/axp/common.c b/drivers/allwinner/axp/common.c
index 143fb0f..f1250b0 100644
--- a/drivers/allwinner/axp/common.c
+++ b/drivers/allwinner/axp/common.c
@@ -48,6 +48,7 @@
 	axp_setbits(0x32, BIT(7));
 }
 
+#if SUNXI_SETUP_REGULATORS == 1
 /*
  * Retrieve the voltage from a given regulator DTB node.
  * Both the regulator-{min,max}-microvolt properties must be present and
@@ -208,3 +209,4 @@
 			axp_setbits(0x11, BIT(7));
 	}
 }
+#endif	/* SUNXI_SETUP_REGULATORS */
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
new file mode 100644
index 0000000..4cbc0f7
--- /dev/null
+++ b/drivers/clk/clk.c
@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ * Author(s): Ludovic Barre, <ludovic.barre@st.com> for STMicroelectronics.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include <drivers/clk.h>
+
+static const struct clk_ops *ops;
+
+int clk_enable(unsigned long id)
+{
+	assert((ops != NULL) && (ops->enable != NULL));
+
+	return ops->enable(id);
+}
+
+void clk_disable(unsigned long id)
+{
+	assert((ops != NULL) && (ops->disable != NULL));
+
+	ops->disable(id);
+}
+
+unsigned long clk_get_rate(unsigned long id)
+{
+	assert((ops != NULL) && (ops->get_rate != NULL));
+
+	return ops->get_rate(id);
+}
+
+int clk_get_parent(unsigned long id)
+{
+	assert((ops != NULL) && (ops->get_parent != NULL));
+
+	return ops->get_parent(id);
+}
+
+bool clk_is_enabled(unsigned long id)
+{
+	assert((ops != NULL) && (ops->is_enabled != NULL));
+
+	return ops->is_enabled(id);
+}
+
+/*
+ * Initialize the clk. The fields in the provided clk
+ * ops pointer must be valid.
+ */
+void clk_register(const struct clk_ops *ops_ptr)
+{
+	assert((ops_ptr != NULL) &&
+	       (ops_ptr->enable != NULL) &&
+	       (ops_ptr->disable != NULL) &&
+	       (ops_ptr->get_rate != NULL) &&
+	       (ops_ptr->get_parent != NULL) &&
+	       (ops_ptr->is_enabled != NULL));
+
+	ops = ops_ptr;
+}
diff --git a/drivers/st/clk/stm32mp1_clk.c b/drivers/st/clk/stm32mp1_clk.c
index 5d4b8fb..684ab1a 100644
--- a/drivers/st/clk/stm32mp1_clk.c
+++ b/drivers/st/clk/stm32mp1_clk.c
@@ -17,6 +17,7 @@
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/generic_delay_timer.h>
 #include <drivers/st/stm32mp_clkfunc.h>
@@ -1157,17 +1158,19 @@
 	stm32mp1_clk_unlock(&refcount_lock);
 }
 
-void stm32mp_clk_enable(unsigned long id)
+static int stm32mp_clk_enable(unsigned long id)
 {
 	__stm32mp1_clk_enable(id, true);
+
+	return 0;
 }
 
-void stm32mp_clk_disable(unsigned long id)
+static void stm32mp_clk_disable(unsigned long id)
 {
 	__stm32mp1_clk_disable(id, true);
 }
 
-bool stm32mp_clk_is_enabled(unsigned long id)
+static bool stm32mp_clk_is_enabled(unsigned long id)
 {
 	int i;
 
@@ -1183,15 +1186,55 @@
 	return __clk_is_enabled(gate_ref(i));
 }
 
-unsigned long stm32mp_clk_get_rate(unsigned long id)
+static unsigned long stm32mp_clk_get_rate(unsigned long id)
 {
+	uintptr_t rcc_base = stm32mp_rcc_base();
 	int p = stm32mp1_clk_get_parent(id);
+	uint32_t prescaler, timpre;
+	unsigned long parent_rate;
 
 	if (p < 0) {
 		return 0;
 	}
 
+	parent_rate = get_clock_rate(p);
+
+	switch (id) {
+	case TIM2_K:
+	case TIM3_K:
+	case TIM4_K:
+	case TIM5_K:
+	case TIM6_K:
+	case TIM7_K:
+	case TIM12_K:
+	case TIM13_K:
+	case TIM14_K:
+		prescaler = mmio_read_32(rcc_base + RCC_APB1DIVR) &
+			    RCC_APBXDIV_MASK;
+		timpre = mmio_read_32(rcc_base + RCC_TIMG1PRER) &
+			 RCC_TIMGXPRER_TIMGXPRE;
+		break;
+
+	case TIM1_K:
+	case TIM8_K:
+	case TIM15_K:
+	case TIM16_K:
+	case TIM17_K:
+		prescaler = mmio_read_32(rcc_base + RCC_APB2DIVR) &
+			    RCC_APBXDIV_MASK;
+		timpre = mmio_read_32(rcc_base + RCC_TIMG2PRER) &
+			 RCC_TIMGXPRER_TIMGXPRE;
+		break;
+
-	return get_clock_rate(p);
+	default:
+		return parent_rate;
+	}
+
+	if (prescaler == 0U) {
+		return parent_rate;
+	}
+
+	return parent_rate * (timpre + 1U) * 2U;
 }
 
 static void stm32mp1_ls_osc_set(bool enable, uint32_t offset, uint32_t mask_on)
@@ -2264,11 +2307,21 @@
 	}
 }
 
+static const struct clk_ops stm32mp_clk_ops = {
+	.enable		= stm32mp_clk_enable,
+	.disable	= stm32mp_clk_disable,
+	.is_enabled	= stm32mp_clk_is_enabled,
+	.get_rate	= stm32mp_clk_get_rate,
+	.get_parent	= stm32mp1_clk_get_parent,
+};
+
 int stm32mp1_clk_probe(void)
 {
 	stm32mp1_osc_init();
 
 	sync_earlyboot_clocks_state();
 
+	clk_register(&stm32mp_clk_ops);
+
 	return 0;
 }
diff --git a/drivers/st/clk/stm32mp_clkfunc.c b/drivers/st/clk/stm32mp_clkfunc.c
index 0db0dee..c83b8ad 100644
--- a/drivers/st/clk/stm32mp_clkfunc.c
+++ b/drivers/st/clk/stm32mp_clkfunc.c
@@ -11,6 +11,7 @@
 #include <platform_def.h>
 
 #include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
 #include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stm32mp_clkfunc.h>
 
@@ -325,5 +326,5 @@
 		return 0UL;
 	}
 
-	return stm32mp_clk_get_rate((unsigned long)clk_id);
+	return clk_get_rate((unsigned long)clk_id);
 }
diff --git a/drivers/st/crypto/stm32_hash.c b/drivers/st/crypto/stm32_hash.c
index 317fd9e..6a1d476 100644
--- a/drivers/st/crypto/stm32_hash.c
+++ b/drivers/st/crypto/stm32_hash.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2019-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,6 +14,7 @@
 
 #include <arch_helpers.h>
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/st/stm32_hash.h>
 #include <drivers/st/stm32mp_reset.h>
@@ -189,7 +190,7 @@
 		return 0;
 	}
 
-	stm32mp_clk_enable(stm32_hash.clock);
+	clk_enable(stm32_hash.clock);
 
 	if (stm32_remain.length != 0U) {
 		uint32_t copysize;
@@ -231,7 +232,7 @@
 	}
 
 exit:
-	stm32mp_clk_disable(stm32_hash.clock);
+	clk_disable(stm32_hash.clock);
 
 	return ret;
 }
@@ -240,12 +241,12 @@
 {
 	int ret;
 
-	stm32mp_clk_enable(stm32_hash.clock);
+	clk_enable(stm32_hash.clock);
 
 	if (stm32_remain.length != 0U) {
 		ret = hash_write_data(stm32_remain.buffer);
 		if (ret != 0) {
-			stm32mp_clk_disable(stm32_hash.clock);
+			clk_disable(stm32_hash.clock);
 			return ret;
 		}
 
@@ -260,7 +261,7 @@
 
 	ret = hash_get_digest(digest);
 
-	stm32mp_clk_disable(stm32_hash.clock);
+	clk_disable(stm32_hash.clock);
 
 	return ret;
 }
@@ -280,11 +281,11 @@
 
 void stm32_hash_init(enum stm32_hash_algo_mode mode)
 {
-	stm32mp_clk_enable(stm32_hash.clock);
+	clk_enable(stm32_hash.clock);
 
 	hash_hw_init(mode);
 
-	stm32mp_clk_disable(stm32_hash.clock);
+	clk_disable(stm32_hash.clock);
 
 	zeromem(&stm32_remain, sizeof(stm32_remain));
 }
@@ -321,7 +322,7 @@
 	stm32_hash.base = hash_info.base;
 	stm32_hash.clock = hash_info.clock;
 
-	stm32mp_clk_enable(stm32_hash.clock);
+	clk_enable(stm32_hash.clock);
 
 	if (hash_info.reset >= 0) {
 		uint32_t id = (uint32_t)hash_info.reset;
@@ -335,7 +336,7 @@
 		}
 	}
 
-	stm32mp_clk_disable(stm32_hash.clock);
+	clk_disable(stm32_hash.clock);
 
 	return 0;
 }
diff --git a/drivers/st/ddr/stm32mp1_ddr.c b/drivers/st/ddr/stm32mp1_ddr.c
index 7d89d02..3db47c4 100644
--- a/drivers/st/ddr/stm32mp1_ddr.c
+++ b/drivers/st/ddr/stm32mp1_ddr.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2018-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (C) 2018-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
  */
@@ -12,6 +12,7 @@
 #include <arch.h>
 #include <arch_helpers.h>
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/st/stm32mp_pmic.h>
 #include <drivers/st/stm32mp1_ddr.h>
@@ -627,7 +628,7 @@
 	 */
 
 	/* Change Bypass Mode Frequency Range */
-	if (stm32mp_clk_get_rate(DDRPHYC) < 100000000U) {
+	if (clk_get_rate(DDRPHYC) < 100000000U) {
 		mmio_clrbits_32((uintptr_t)&priv->phy->dllgcr,
 				DDRPHYC_DLLGCR_BPS200);
 	} else {
diff --git a/drivers/st/ddr/stm32mp1_ram.c b/drivers/st/ddr/stm32mp1_ram.c
index b21c894..064e318 100644
--- a/drivers/st/ddr/stm32mp1_ram.c
+++ b/drivers/st/ddr/stm32mp1_ram.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2018-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (C) 2018-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
  */
@@ -13,6 +13,7 @@
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
 #include <drivers/st/stm32mp1_ddr.h>
 #include <drivers/st/stm32mp1_ddr_helpers.h>
 #include <drivers/st/stm32mp1_ram.h>
@@ -29,7 +30,7 @@
 
 	ddr_enable_clock();
 
-	ddrphy_clk = stm32mp_clk_get_rate(DDRPHYC);
+	ddrphy_clk = clk_get_rate(DDRPHYC);
 
 	VERBOSE("DDR: mem_speed (%d kHz), RCC %ld kHz\n",
 		mem_speed, ddrphy_clk / 1000U);
diff --git a/drivers/st/fmc/stm32_fmc2_nand.c b/drivers/st/fmc/stm32_fmc2_nand.c
index 453069b..e9ab6da 100644
--- a/drivers/st/fmc/stm32_fmc2_nand.c
+++ b/drivers/st/fmc/stm32_fmc2_nand.c
@@ -14,6 +14,7 @@
 #include <platform_def.h>
 
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/raw_nand.h>
 #include <drivers/st/stm32_fmc2_nand.h>
@@ -162,7 +163,7 @@
 static void stm32_fmc2_nand_setup_timing(void)
 {
 	struct stm32_fmc2_nand_timings tims;
-	unsigned long hclk = stm32mp_clk_get_rate(stm32_fmc2.clock_id);
+	unsigned long hclk = clk_get_rate(stm32_fmc2.clock_id);
 	unsigned long hclkp = FMC2_PSEC_PER_MSEC / (hclk / 1000U);
 	unsigned long timing, tar, tclr, thiz, twait;
 	unsigned long tset_mem, tset_att, thold_mem, thold_att;
@@ -909,7 +910,7 @@
 	}
 
 	/* Enable Clock */
-	stm32mp_clk_enable(stm32_fmc2.clock_id);
+	clk_enable(stm32_fmc2.clock_id);
 
 	/* Reset IP */
 	ret = stm32mp_reset_assert(stm32_fmc2.reset_id, TIMEOUT_US_1_MS);
diff --git a/drivers/st/gpio/stm32_gpio.c b/drivers/st/gpio/stm32_gpio.c
index 51276f4..5c54762 100644
--- a/drivers/st/gpio/stm32_gpio.c
+++ b/drivers/st/gpio/stm32_gpio.c
@@ -14,6 +14,7 @@
 
 #include <common/bl_common.h>
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stm32mp_clkfunc.h>
 #include <lib/mmio.h>
@@ -208,7 +209,7 @@
 
 	assert(pin <= GPIO_PIN_MAX);
 
-	stm32mp_clk_enable(clock);
+	clk_enable(clock);
 
 	mmio_clrbits_32(base + GPIO_MODE_OFFSET,
 			((uint32_t)GPIO_MODE_MASK << (pin << 1)));
@@ -254,7 +255,7 @@
 	VERBOSE("GPIO %u mode alternate high to 0x%x\n", bank,
 		mmio_read_32(base + GPIO_AFRH_OFFSET));
 
-	stm32mp_clk_disable(clock);
+	clk_disable(clock);
 
 	if (status == DT_SECURE) {
 		stm32mp_register_secure_gpio(bank, pin);
@@ -273,7 +274,7 @@
 
 	assert(pin <= GPIO_PIN_MAX);
 
-	stm32mp_clk_enable(clock);
+	clk_enable(clock);
 
 	if (secure) {
 		mmio_setbits_32(base + GPIO_SECR_OFFSET, BIT(pin));
@@ -281,7 +282,7 @@
 		mmio_clrbits_32(base + GPIO_SECR_OFFSET, BIT(pin));
 	}
 
-	stm32mp_clk_disable(clock);
+	clk_disable(clock);
 }
 
 void set_gpio_reset_cfg(uint32_t bank, uint32_t pin)
diff --git a/drivers/st/i2c/stm32_i2c.c b/drivers/st/i2c/stm32_i2c.c
index ed88052..bf6c3ee 100644
--- a/drivers/st/i2c/stm32_i2c.c
+++ b/drivers/st/i2c/stm32_i2c.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -13,6 +13,7 @@
 #include <platform_def.h>
 
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stm32_i2c.h>
@@ -158,7 +159,7 @@
 
 	hi2c->i2c_state = I2C_STATE_BUSY;
 
-	stm32mp_clk_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	/* Disable the selected I2C peripheral */
 	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
@@ -220,11 +221,11 @@
 						I2C_ANALOGFILTER_DISABLE);
 	if (rc != 0) {
 		ERROR("Cannot initialize I2C analog filter (%d)\n", rc);
-		stm32mp_clk_disable(hi2c->clock);
+		clk_disable(hi2c->clock);
 		return rc;
 	}
 
-	stm32mp_clk_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
@@ -548,7 +549,7 @@
 		return -EINVAL;
 	}
 
-	stm32mp_clk_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	hi2c->lock = 1;
 
@@ -648,7 +649,7 @@
 
 bail:
 	hi2c->lock = 0;
-	stm32mp_clk_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
@@ -729,7 +730,7 @@
 		return  -EINVAL;
 	}
 
-	stm32mp_clk_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	hi2c->lock = 1;
 
@@ -817,7 +818,7 @@
 
 bail:
 	hi2c->lock = 0;
-	stm32mp_clk_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
@@ -882,7 +883,7 @@
 		return rc;
 	}
 
-	stm32mp_clk_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	hi2c->lock = 1;
 	hi2c->i2c_mode = I2C_MODE_NONE;
@@ -974,7 +975,7 @@
 
 bail:
 	hi2c->lock = 0;
-	stm32mp_clk_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
diff --git a/drivers/st/iwdg/stm32_iwdg.c b/drivers/st/iwdg/stm32_iwdg.c
index c052b4d..74451d7 100644
--- a/drivers/st/iwdg/stm32_iwdg.c
+++ b/drivers/st/iwdg/stm32_iwdg.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -15,6 +15,7 @@
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <drivers/arm/gicv2.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/st/stm32_iwdg.h>
 #include <drivers/st/stm32mp_clkfunc.h>
@@ -61,12 +62,12 @@
 
 		/* 0x00000000 is not a valid address for IWDG peripherals */
 		if (iwdg->base != 0U) {
-			stm32mp_clk_enable(iwdg->clock);
+			clk_enable(iwdg->clock);
 
 			mmio_write_32(iwdg->base + IWDG_KR_OFFSET,
 				      IWDG_KR_RELOAD_KEY);
 
-			stm32mp_clk_disable(iwdg->clock);
+			clk_disable(iwdg->clock);
 		}
 	}
 }
diff --git a/drivers/st/mmc/stm32_sdmmc2.c b/drivers/st/mmc/stm32_sdmmc2.c
index d3adeab..dbdaa54 100644
--- a/drivers/st/mmc/stm32_sdmmc2.c
+++ b/drivers/st/mmc/stm32_sdmmc2.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2018-2022, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -8,13 +8,10 @@
 #include <errno.h>
 #include <string.h>
 
-#include <libfdt.h>
-
-#include <platform_def.h>
-
 #include <arch.h>
 #include <arch_helpers.h>
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/mmc.h>
 #include <drivers/st/stm32_gpio.h>
@@ -22,8 +19,11 @@
 #include <drivers/st/stm32mp_reset.h>
 #include <lib/mmio.h>
 #include <lib/utils.h>
+#include <libfdt.h>
 #include <plat/common/platform.h>
 
+#include <platform_def.h>
+
 /* Registers offsets */
 #define SDMMC_POWER			0x00U
 #define SDMMC_CLKCR			0x04U
@@ -50,6 +50,7 @@
 
 /* SDMMC power control register */
 #define SDMMC_POWER_PWRCTRL		GENMASK(1, 0)
+#define SDMMC_POWER_PWRCTRL_PWR_CYCLE	BIT(1)
 #define SDMMC_POWER_DIRPOL		BIT(4)
 
 /* SDMMC clock control register */
@@ -117,6 +118,13 @@
 #define TIMEOUT_US_10_MS		10000U
 #define TIMEOUT_US_1_S			1000000U
 
+/* Power cycle delays in ms */
+#define VCC_POWER_OFF_DELAY		2
+#define VCC_POWER_ON_DELAY		2
+#define POWER_CYCLE_DELAY		2
+#define POWER_OFF_DELAY			2
+#define POWER_ON_DELAY			1
+
 #define DT_SDMMC2_COMPAT		"st,stm32-sdmmc2"
 
 static void stm32_sdmmc2_init(void);
@@ -149,11 +157,37 @@
 	uint32_t clock_div;
 	uint32_t freq = STM32MP_MMC_INIT_FREQ;
 	uintptr_t base = sdmmc2_params.reg_base;
+	int ret;
 
 	if (sdmmc2_params.max_freq != 0U) {
 		freq = MIN(sdmmc2_params.max_freq, freq);
 	}
 
+	if (sdmmc2_params.vmmc_regu != NULL) {
+		ret = regulator_disable(sdmmc2_params.vmmc_regu);
+		if (ret < 0) {
+			panic();
+		}
+	}
+
+	mdelay(VCC_POWER_OFF_DELAY);
+
+	mmio_write_32(base + SDMMC_POWER,
+		      SDMMC_POWER_PWRCTRL_PWR_CYCLE | sdmmc2_params.dirpol);
+	mdelay(POWER_CYCLE_DELAY);
+
+	if (sdmmc2_params.vmmc_regu != NULL) {
+		ret = regulator_enable(sdmmc2_params.vmmc_regu);
+		if (ret < 0) {
+			panic();
+		}
+	}
+
+	mdelay(VCC_POWER_ON_DELAY);
+
+	mmio_write_32(base + SDMMC_POWER, sdmmc2_params.dirpol);
+	mdelay(POWER_OFF_DELAY);
+
 	clock_div = div_round_up(sdmmc2_params.clk_rate, freq * 2U);
 
 	mmio_write_32(base + SDMMC_CLKCR, SDMMC_CLKCR_HWFC_EN | clock_div |
@@ -163,7 +197,7 @@
 	mmio_write_32(base + SDMMC_POWER,
 		      SDMMC_POWER_PWRCTRL | sdmmc2_params.dirpol);
 
-	mdelay(1);
+	mdelay(POWER_ON_DELAY);
 }
 
 static int stm32_sdmmc2_stop_transfer(void)
@@ -689,6 +723,8 @@
 		sdmmc2_params.max_freq = fdt32_to_cpu(*cuint);
 	}
 
+	sdmmc2_params.vmmc_regu = regulator_get_by_supply_name(fdt, sdmmc_node, "vmmc");
+
 	return 0;
 }
 
@@ -709,12 +745,14 @@
 
 	memcpy(&sdmmc2_params, params, sizeof(struct stm32_sdmmc2_params));
 
+	sdmmc2_params.vmmc_regu = NULL;
+
 	if (stm32_sdmmc2_dt_get_config() != 0) {
 		ERROR("%s: DT error\n", __func__);
 		return -ENOMEM;
 	}
 
-	stm32mp_clk_enable(sdmmc2_params.clock_id);
+	clk_enable(sdmmc2_params.clock_id);
 
 	rc = stm32mp_reset_assert(sdmmc2_params.reset_id, TIMEOUT_US_1_MS);
 	if (rc != 0) {
@@ -727,7 +765,7 @@
 	}
 	mdelay(1);
 
-	sdmmc2_params.clk_rate = stm32mp_clk_get_rate(sdmmc2_params.clock_id);
+	sdmmc2_params.clk_rate = clk_get_rate(sdmmc2_params.clock_id);
 	sdmmc2_params.device_info->ocr_voltage = OCR_3_2_3_3 | OCR_3_3_3_4;
 
 	return mmc_init(&stm32_sdmmc2_ops, sdmmc2_params.clk_rate,
diff --git a/drivers/st/pmic/stm32mp_pmic.c b/drivers/st/pmic/stm32mp_pmic.c
index be410a1..6a30dce 100644
--- a/drivers/st/pmic/stm32mp_pmic.c
+++ b/drivers/st/pmic/stm32mp_pmic.c
@@ -4,54 +4,63 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <assert.h>
 #include <errno.h>
 
-#include <libfdt.h>
-
-#include <platform_def.h>
-
 #include <common/debug.h>
 #include <drivers/delay_timer.h>
+#include <drivers/st/regulator.h>
 #include <drivers/st/stm32_i2c.h>
 #include <drivers/st/stm32mp_pmic.h>
 #include <drivers/st/stpmic1.h>
 #include <lib/mmio.h>
 #include <lib/utils_def.h>
-
-#define STPMIC1_LDO12356_OUTPUT_MASK	(uint8_t)(GENMASK(6, 2))
-#define STPMIC1_LDO12356_OUTPUT_SHIFT	2
-#define STPMIC1_LDO3_MODE		(uint8_t)(BIT(7))
-#define STPMIC1_LDO3_DDR_SEL		31U
-#define STPMIC1_LDO3_1800000		(9U << STPMIC1_LDO12356_OUTPUT_SHIFT)
+#include <libfdt.h>
 
-#define STPMIC1_BUCK_OUTPUT_SHIFT	2
-#define STPMIC1_BUCK3_1V8		(39U << STPMIC1_BUCK_OUTPUT_SHIFT)
+#include <platform_def.h>
 
-#define STPMIC1_DEFAULT_START_UP_DELAY_MS	1
+#define PMIC_NODE_NOT_FOUND	1
 
 static struct i2c_handle_s i2c_handle;
 static uint32_t pmic_i2c_addr;
 
+static int register_pmic(void);
+
 static int dt_get_pmic_node(void *fdt)
 {
-	return fdt_node_offset_by_compatible(fdt, -1, "st,stpmic1");
+	static int node = -FDT_ERR_BADOFFSET;
+
+	if (node == -FDT_ERR_BADOFFSET) {
+		node = fdt_node_offset_by_compatible(fdt, -1, "st,stpmic1");
+	}
+
+	return node;
 }
 
 int dt_pmic_status(void)
 {
+	static int status = -FDT_ERR_BADVALUE;
 	int node;
 	void *fdt;
 
+	if (status != -FDT_ERR_BADVALUE) {
+		return status;
+	}
+
 	if (fdt_get_address(&fdt) == 0) {
 		return -ENOENT;
 	}
 
 	node = dt_get_pmic_node(fdt);
 	if (node <= 0) {
-		return -FDT_ERR_NOTFOUND;
+		status = -FDT_ERR_NOTFOUND;
+
+		return status;
 	}
 
-	return fdt_get_status(node);
+	status = (int)fdt_get_status(node);
+
+	return status;
 }
 
 static bool dt_pmic_is_secure(void)
@@ -65,125 +74,49 @@
 
 /*
  * Get PMIC and its I2C bus configuration from the device tree.
- * Return 0 on success, negative on error, 1 if no PMIC node is found.
+ * Return 0 on success, negative on error, 1 if no PMIC node is defined.
  */
 static int dt_pmic_i2c_config(struct dt_node_info *i2c_info,
 			      struct stm32_i2c_init_s *init)
 {
-	int pmic_node, i2c_node;
+	static int i2c_node = -FDT_ERR_NOTFOUND;
 	void *fdt;
-	const fdt32_t *cuint;
 
 	if (fdt_get_address(&fdt) == 0) {
-		return -ENOENT;
-	}
-
-	pmic_node = dt_get_pmic_node(fdt);
-	if (pmic_node < 0) {
-		return 1;
-	}
-
-	cuint = fdt_getprop(fdt, pmic_node, "reg", NULL);
-	if (cuint == NULL) {
 		return -FDT_ERR_NOTFOUND;
 	}
 
-	pmic_i2c_addr = fdt32_to_cpu(*cuint) << 1;
-	if (pmic_i2c_addr > UINT16_MAX) {
-		return -EINVAL;
-	}
-
-	i2c_node = fdt_parent_offset(fdt, pmic_node);
-	if (i2c_node < 0) {
-		return -FDT_ERR_NOTFOUND;
-	}
-
-	dt_fill_device_info(i2c_info, i2c_node);
-	if (i2c_info->base == 0U) {
-		return -FDT_ERR_NOTFOUND;
-	}
-
-	return stm32_i2c_get_setup_from_fdt(fdt, i2c_node, init);
-}
-
-int dt_pmic_configure_boot_on_regulators(void)
-{
-	int pmic_node, regulators_node, regulator_node;
-	void *fdt;
-
-	if (fdt_get_address(&fdt) == 0) {
-		return -ENOENT;
-	}
-
-	pmic_node = dt_get_pmic_node(fdt);
-	if (pmic_node < 0) {
-		return -FDT_ERR_NOTFOUND;
-	}
-
-	regulators_node = fdt_subnode_offset(fdt, pmic_node, "regulators");
-	if (regulators_node < 0) {
-		return -ENOENT;
-	}
-
-	fdt_for_each_subnode(regulator_node, fdt, regulators_node) {
+	if (i2c_node == -FDT_ERR_NOTFOUND) {
+		int pmic_node;
 		const fdt32_t *cuint;
-		const char *node_name = fdt_get_name(fdt, regulator_node, NULL);
-		uint16_t voltage;
-		int status;
 
-#if defined(IMAGE_BL2)
-		if ((fdt_getprop(fdt, regulator_node, "regulator-boot-on",
-				 NULL) == NULL) &&
-		    (fdt_getprop(fdt, regulator_node, "regulator-always-on",
-				 NULL) == NULL)) {
-#else
-		if (fdt_getprop(fdt, regulator_node, "regulator-boot-on",
-				NULL) == NULL) {
-#endif
-			continue;
+		pmic_node = dt_get_pmic_node(fdt);
+		if (pmic_node < 0) {
+			return PMIC_NODE_NOT_FOUND;
 		}
 
-		if (fdt_getprop(fdt, regulator_node, "regulator-pull-down",
-				NULL) != NULL) {
-
-			status = stpmic1_regulator_pull_down_set(node_name);
-			if (status != 0) {
-				return status;
-			}
-		}
-
-		if (fdt_getprop(fdt, regulator_node, "st,mask-reset",
-				NULL) != NULL) {
-
-			status = stpmic1_regulator_mask_reset_set(node_name);
-			if (status != 0) {
-				return status;
-			}
-		}
-
-		cuint = fdt_getprop(fdt, regulator_node,
-				    "regulator-min-microvolt", NULL);
+		cuint = fdt_getprop(fdt, pmic_node, "reg", NULL);
 		if (cuint == NULL) {
-			continue;
+			return -FDT_ERR_NOTFOUND;
 		}
 
-		/* DT uses microvolts, whereas driver awaits millivolts */
-		voltage = (uint16_t)(fdt32_to_cpu(*cuint) / 1000U);
-
-		status = stpmic1_regulator_voltage_set(node_name, voltage);
-		if (status != 0) {
-			return status;
+		pmic_i2c_addr = fdt32_to_cpu(*cuint) << 1;
+		if (pmic_i2c_addr > UINT16_MAX) {
+			return -FDT_ERR_BADVALUE;
 		}
 
-		if (stpmic1_is_regulator_enabled(node_name) == 0U) {
-			status = stpmic1_regulator_enable(node_name);
-			if (status != 0) {
-				return status;
-			}
+		i2c_node = fdt_parent_offset(fdt, pmic_node);
+		if (i2c_node < 0) {
+			return -FDT_ERR_NOTFOUND;
 		}
 	}
 
-	return 0;
+	dt_fill_device_info(i2c_info, i2c_node);
+	if (i2c_info->base == 0U) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return stm32_i2c_get_setup_from_fdt(fdt, i2c_node, init);
 }
 
 bool initialize_pmic_i2c(void)
@@ -251,8 +184,6 @@
 
 void initialize_pmic(void)
 {
-	unsigned long pmic_version;
-
 	if (!initialize_pmic_i2c()) {
 		VERBOSE("No PMIC\n");
 		return;
@@ -260,70 +191,77 @@
 
 	register_pmic_shared_peripherals();
 
+	if (register_pmic() < 0) {
+		panic();
+	}
+
+	if (stpmic1_powerctrl_on() < 0) {
+		panic();
+	}
+
+}
+
+#if DEBUG
+void print_pmic_info_and_debug(void)
+{
+	unsigned long pmic_version;
+
 	if (stpmic1_get_version(&pmic_version) != 0) {
 		ERROR("Failed to access PMIC\n");
 		panic();
 	}
 
 	INFO("PMIC version = 0x%02lx\n", pmic_version);
-	stpmic1_dump_regulators();
-
-#if defined(IMAGE_BL2)
-	if (dt_pmic_configure_boot_on_regulators() != 0) {
-		panic();
-	};
-#endif
 }
+#endif
 
 int pmic_ddr_power_init(enum ddr_type ddr_type)
 {
-	bool buck3_at_1v8 = false;
-	uint8_t read_val;
 	int status;
+	uint16_t buck3_min_mv;
+	struct rdev *buck2, *buck3, *ldo3, *vref;
 
-	switch (ddr_type) {
-	case STM32MP_DDR3:
-		/* Set LDO3 to sync mode */
-		status = stpmic1_register_read(LDO3_CONTROL_REG, &read_val);
-		if (status != 0) {
-			return status;
-		}
+	buck2 = regulator_get_by_name("buck2");
+	if (buck2 == NULL) {
+		return -ENOENT;
+	}
 
-		read_val &= ~STPMIC1_LDO3_MODE;
-		read_val &= ~STPMIC1_LDO12356_OUTPUT_MASK;
-		read_val |= STPMIC1_LDO3_DDR_SEL <<
-			    STPMIC1_LDO12356_OUTPUT_SHIFT;
+	ldo3 = regulator_get_by_name("ldo3");
+	if (ldo3 == NULL) {
+		return -ENOENT;
+	}
 
-		status = stpmic1_register_write(LDO3_CONTROL_REG, read_val);
+	vref = regulator_get_by_name("vref_ddr");
+	if (vref == NULL) {
+		return -ENOENT;
+	}
+
+	switch (ddr_type) {
+	case STM32MP_DDR3:
+		status = regulator_set_flag(ldo3, REGUL_SINK_SOURCE);
 		if (status != 0) {
 			return status;
 		}
 
-		status = stpmic1_regulator_voltage_set("buck2", 1350);
+		status = regulator_set_min_voltage(buck2);
 		if (status != 0) {
 			return status;
 		}
 
-		status = stpmic1_regulator_enable("buck2");
+		status = regulator_enable(buck2);
 		if (status != 0) {
 			return status;
 		}
 
-		mdelay(STPMIC1_DEFAULT_START_UP_DELAY_MS);
-
-		status = stpmic1_regulator_enable("vref_ddr");
+		status = regulator_enable(vref);
 		if (status != 0) {
 			return status;
 		}
 
-		mdelay(STPMIC1_DEFAULT_START_UP_DELAY_MS);
-
-		status = stpmic1_regulator_enable("ldo3");
+		status = regulator_enable(ldo3);
 		if (status != 0) {
 			return status;
 		}
-
-		mdelay(STPMIC1_DEFAULT_START_UP_DELAY_MS);
 		break;
 
 	case STM32MP_LPDDR2:
@@ -333,62 +271,215 @@
 		 * Set LDO3 to bypass mode if BUCK3 = 1.8V
 		 * Set LDO3 to normal mode if BUCK3 != 1.8V
 		 */
-		status = stpmic1_register_read(BUCK3_CONTROL_REG, &read_val);
-		if (status != 0) {
-			return status;
+		buck3 = regulator_get_by_name("buck3");
+		if (buck3 == NULL) {
+			return -ENOENT;
 		}
 
-		if ((read_val & STPMIC1_BUCK3_1V8) == STPMIC1_BUCK3_1V8) {
-			buck3_at_1v8 = true;
+		regulator_get_range(buck3, &buck3_min_mv, NULL);
+
+		if (buck3_min_mv != 1800) {
+			status = regulator_set_min_voltage(ldo3);
+			if (status != 0) {
+				return status;
+			}
+		} else {
+			status = regulator_set_flag(ldo3, REGUL_ENABLE_BYPASS);
+			if (status != 0) {
+				return status;
+			}
 		}
 
-		status = stpmic1_register_read(LDO3_CONTROL_REG, &read_val);
+		status = regulator_set_min_voltage(buck2);
 		if (status != 0) {
 			return status;
 		}
 
-		read_val &= ~STPMIC1_LDO3_MODE;
-		read_val &= ~STPMIC1_LDO12356_OUTPUT_MASK;
-		read_val |= STPMIC1_LDO3_1800000;
-		if (buck3_at_1v8) {
-			read_val |= STPMIC1_LDO3_MODE;
-		}
-
-		status = stpmic1_register_write(LDO3_CONTROL_REG, read_val);
+		status = regulator_enable(ldo3);
 		if (status != 0) {
 			return status;
 		}
 
-		status = stpmic1_regulator_voltage_set("buck2", 1200);
+		status = regulator_enable(buck2);
 		if (status != 0) {
 			return status;
 		}
 
-		status = stpmic1_regulator_enable("ldo3");
+		status = regulator_enable(vref);
 		if (status != 0) {
 			return status;
 		}
+		break;
 
-		mdelay(STPMIC1_DEFAULT_START_UP_DELAY_MS);
+	default:
+		break;
+	};
 
-		status = stpmic1_regulator_enable("buck2");
-		if (status != 0) {
-			return status;
-		}
+	return 0;
+}
 
-		mdelay(STPMIC1_DEFAULT_START_UP_DELAY_MS);
+enum {
+	STPMIC1_BUCK1 = 0,
+	STPMIC1_BUCK2,
+	STPMIC1_BUCK3,
+	STPMIC1_BUCK4,
+	STPMIC1_LDO1,
+	STPMIC1_LDO2,
+	STPMIC1_LDO3,
+	STPMIC1_LDO4,
+	STPMIC1_LDO5,
+	STPMIC1_LDO6,
+	STPMIC1_VREF_DDR,
+	STPMIC1_BOOST,
+	STPMIC1_VBUS_OTG,
+	STPMIC1_SW_OUT,
+};
 
-		status = stpmic1_regulator_enable("vref_ddr");
-		if (status != 0) {
-			return status;
-		}
+static int pmic_set_state(const struct regul_description *desc, bool enable)
+{
+	VERBOSE("%s: set state to %u\n", desc->node_name, enable);
 
-		mdelay(STPMIC1_DEFAULT_START_UP_DELAY_MS);
-		break;
+	if (enable == STATE_ENABLE) {
+		return stpmic1_regulator_enable(desc->node_name);
+	} else {
+		return stpmic1_regulator_disable(desc->node_name);
+	}
+}
+
+static int pmic_get_state(const struct regul_description *desc)
+{
+	VERBOSE("%s: get state\n", desc->node_name);
+
+	return stpmic1_is_regulator_enabled(desc->node_name);
+}
+
+static int pmic_get_voltage(const struct regul_description *desc)
+{
+	VERBOSE("%s: get volt\n", desc->node_name);
+
+	return stpmic1_regulator_voltage_get(desc->node_name);
+}
+
+static int pmic_set_voltage(const struct regul_description *desc, uint16_t mv)
+{
+	VERBOSE("%s: get volt\n", desc->node_name);
+
+	return stpmic1_regulator_voltage_set(desc->node_name, mv);
+}
+
+static int pmic_list_voltages(const struct regul_description *desc,
+			      const uint16_t **levels, size_t *count)
+{
+	VERBOSE("%s: list volt\n", desc->node_name);
+
+	return stpmic1_regulator_levels_mv(desc->node_name, levels, count);
+}
+
+static int pmic_set_flag(const struct regul_description *desc, uint16_t flag)
+{
+	VERBOSE("%s: set_flag 0x%x\n", desc->node_name, flag);
+
+	switch (flag) {
+	case REGUL_OCP:
+		return stpmic1_regulator_icc_set(desc->node_name);
+
+	case REGUL_ACTIVE_DISCHARGE:
+		return stpmic1_active_discharge_mode_set(desc->node_name);
+
+	case REGUL_PULL_DOWN:
+		return stpmic1_regulator_pull_down_set(desc->node_name);
+
+	case REGUL_MASK_RESET:
+		return stpmic1_regulator_mask_reset_set(desc->node_name);
+
+	case REGUL_SINK_SOURCE:
+		return stpmic1_regulator_sink_mode_set(desc->node_name);
+
+	case REGUL_ENABLE_BYPASS:
+		return stpmic1_regulator_bypass_mode_set(desc->node_name);
 
 	default:
-		break;
-	};
+		return -EINVAL;
+	}
+}
+
+struct regul_ops pmic_ops = {
+	.set_state = pmic_set_state,
+	.get_state = pmic_get_state,
+	.set_voltage = pmic_set_voltage,
+	.get_voltage = pmic_get_voltage,
+	.list_voltages = pmic_list_voltages,
+	.set_flag = pmic_set_flag,
+};
+
+#define DEFINE_REGU(name) { \
+	.node_name = name, \
+	.ops = &pmic_ops, \
+	.driver_data = NULL, \
+	.enable_ramp_delay = 1000, \
+}
+
+static const struct regul_description pmic_regs[] = {
+	[STPMIC1_BUCK1] = DEFINE_REGU("buck1"),
+	[STPMIC1_BUCK2] = DEFINE_REGU("buck2"),
+	[STPMIC1_BUCK3] = DEFINE_REGU("buck3"),
+	[STPMIC1_BUCK4] = DEFINE_REGU("buck4"),
+	[STPMIC1_LDO1] = DEFINE_REGU("ldo1"),
+	[STPMIC1_LDO2] = DEFINE_REGU("ldo2"),
+	[STPMIC1_LDO3] = DEFINE_REGU("ldo3"),
+	[STPMIC1_LDO4] = DEFINE_REGU("ldo4"),
+	[STPMIC1_LDO5] = DEFINE_REGU("ldo5"),
+	[STPMIC1_LDO6] = DEFINE_REGU("ldo6"),
+	[STPMIC1_VREF_DDR] = DEFINE_REGU("vref_ddr"),
+	[STPMIC1_BOOST] = DEFINE_REGU("boost"),
+	[STPMIC1_VBUS_OTG] = DEFINE_REGU("pwr_sw1"),
+	[STPMIC1_SW_OUT] = DEFINE_REGU("pwr_sw2"),
+};
+
+#define NB_REG ARRAY_SIZE(pmic_regs)
+
+static int register_pmic(void)
+{
+	void *fdt;
+	int pmic_node, regulators_node, subnode;
+
+	VERBOSE("Register pmic\n");
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	pmic_node = dt_get_pmic_node(fdt);
+	if (pmic_node < 0) {
+		return pmic_node;
+	}
+
+	regulators_node = fdt_subnode_offset(fdt, pmic_node, "regulators");
+	if (regulators_node < 0) {
+		return -ENOENT;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, regulators_node) {
+		const char *reg_name = fdt_get_name(fdt, subnode, NULL);
+		const struct regul_description *desc;
+		unsigned int i;
+		int ret;
+
+		for (i = 0; i < NB_REG; i++) {
+			desc = &pmic_regs[i];
+			if (strcmp(desc->node_name, reg_name) == 0) {
+				break;
+			}
+		}
+		assert(i < NB_REG);
+
+		ret = regulator_register(desc, subnode);
+		if (ret != 0) {
+			WARN("%s:%d failed to register %s\n", __func__,
+			     __LINE__, reg_name);
+			return ret;
+		}
+	}
 
 	return 0;
 }
diff --git a/drivers/st/pmic/stpmic1.c b/drivers/st/pmic/stpmic1.c
index 0a35df3..37eb50b 100644
--- a/drivers/st/pmic/stpmic1.c
+++ b/drivers/st/pmic/stpmic1.c
@@ -23,10 +23,17 @@
 	uint8_t pull_down;
 	uint8_t mask_reset_reg;
 	uint8_t mask_reset;
+	uint8_t icc_reg;
+	uint8_t icc_mask;
 };
 
 static struct i2c_handle_s *pmic_i2c_handle;
 static uint16_t pmic_i2c_addr;
+/*
+ * Special mode corresponds to LDO3 in sink source mode or in bypass mode.
+ * LDO3 doesn't switch back from special to normal mode.
+ */
+static bool ldo3_special_mode;
 
 /* Voltage tables in mV */
 static const uint16_t buck1_voltage_table[] = {
@@ -347,10 +354,13 @@
 	3300,
 	3300,
 	3300,
-	500,
-	0xFFFF, /* VREFDDR */
 };
 
+/* Special mode table is used for sink source OR bypass mode */
+static const uint16_t ldo3_special_mode_table[] = {
+	0,
+};
+
 static const uint16_t ldo5_voltage_table[] = {
 	1700,
 	1700,
@@ -421,6 +431,10 @@
 	3300,
 };
 
+static const uint16_t fixed_5v_voltage_table[] = {
+	5000,
+};
+
 /* Table of Regulators in PMIC SoC */
 static const struct regul_struct regulators_table[] = {
 	{
@@ -434,6 +448,8 @@
 		.pull_down	= BUCK1_PULL_DOWN_SHIFT,
 		.mask_reset_reg	= MASK_RESET_BUCK_REG,
 		.mask_reset	= BUCK1_MASK_RESET,
+		.icc_reg	= BUCK_ICC_TURNOFF_REG,
+		.icc_mask	= BUCK1_ICC_SHIFT,
 	},
 	{
 		.dt_node_name	= "buck2",
@@ -446,6 +462,8 @@
 		.pull_down	= BUCK2_PULL_DOWN_SHIFT,
 		.mask_reset_reg	= MASK_RESET_BUCK_REG,
 		.mask_reset	= BUCK2_MASK_RESET,
+		.icc_reg	= BUCK_ICC_TURNOFF_REG,
+		.icc_mask	= BUCK2_ICC_SHIFT,
 	},
 	{
 		.dt_node_name	= "buck3",
@@ -458,6 +476,8 @@
 		.pull_down	= BUCK3_PULL_DOWN_SHIFT,
 		.mask_reset_reg	= MASK_RESET_BUCK_REG,
 		.mask_reset	= BUCK3_MASK_RESET,
+		.icc_reg	= BUCK_ICC_TURNOFF_REG,
+		.icc_mask	= BUCK3_ICC_SHIFT,
 	},
 	{
 		.dt_node_name	= "buck4",
@@ -470,6 +490,8 @@
 		.pull_down	= BUCK4_PULL_DOWN_SHIFT,
 		.mask_reset_reg	= MASK_RESET_BUCK_REG,
 		.mask_reset	= BUCK4_MASK_RESET,
+		.icc_reg	= BUCK_ICC_TURNOFF_REG,
+		.icc_mask	= BUCK4_ICC_SHIFT,
 	},
 	{
 		.dt_node_name	= "ldo1",
@@ -480,6 +502,8 @@
 		.low_power_reg	= LDO1_PWRCTRL_REG,
 		.mask_reset_reg	= MASK_RESET_LDO_REG,
 		.mask_reset	= LDO1_MASK_RESET,
+		.icc_reg	= LDO_ICC_TURNOFF_REG,
+		.icc_mask	= LDO1_ICC_SHIFT,
 	},
 	{
 		.dt_node_name	= "ldo2",
@@ -490,6 +514,8 @@
 		.low_power_reg	= LDO2_PWRCTRL_REG,
 		.mask_reset_reg	= MASK_RESET_LDO_REG,
 		.mask_reset	= LDO2_MASK_RESET,
+		.icc_reg	= LDO_ICC_TURNOFF_REG,
+		.icc_mask	= LDO2_ICC_SHIFT,
 	},
 	{
 		.dt_node_name	= "ldo3",
@@ -500,6 +526,8 @@
 		.low_power_reg	= LDO3_PWRCTRL_REG,
 		.mask_reset_reg	= MASK_RESET_LDO_REG,
 		.mask_reset	= LDO3_MASK_RESET,
+		.icc_reg	= LDO_ICC_TURNOFF_REG,
+		.icc_mask	= LDO3_ICC_SHIFT,
 	},
 	{
 		.dt_node_name	= "ldo4",
@@ -510,6 +538,8 @@
 		.low_power_reg	= LDO4_PWRCTRL_REG,
 		.mask_reset_reg	= MASK_RESET_LDO_REG,
 		.mask_reset	= LDO4_MASK_RESET,
+		.icc_reg	= LDO_ICC_TURNOFF_REG,
+		.icc_mask	= LDO4_ICC_SHIFT,
 	},
 	{
 		.dt_node_name	= "ldo5",
@@ -520,6 +550,8 @@
 		.low_power_reg	= LDO5_PWRCTRL_REG,
 		.mask_reset_reg	= MASK_RESET_LDO_REG,
 		.mask_reset	= LDO5_MASK_RESET,
+		.icc_reg	= LDO_ICC_TURNOFF_REG,
+		.icc_mask	= LDO5_ICC_SHIFT,
 	},
 	{
 		.dt_node_name	= "ldo6",
@@ -530,6 +562,8 @@
 		.low_power_reg	= LDO6_PWRCTRL_REG,
 		.mask_reset_reg	= MASK_RESET_LDO_REG,
 		.mask_reset	= LDO6_MASK_RESET,
+		.icc_reg	= LDO_ICC_TURNOFF_REG,
+		.icc_mask	= LDO6_ICC_SHIFT,
 	},
 	{
 		.dt_node_name	= "vref_ddr",
@@ -541,6 +575,33 @@
 		.mask_reset_reg	= MASK_RESET_LDO_REG,
 		.mask_reset	= VREF_DDR_MASK_RESET,
 	},
+	{
+		.dt_node_name	= "boost",
+		.voltage_table	= fixed_5v_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(fixed_5v_voltage_table),
+		.control_reg	= USB_CONTROL_REG,
+		.enable_mask	= BOOST_ENABLED,
+		.icc_reg	= BUCK_ICC_TURNOFF_REG,
+		.icc_mask	= BOOST_ICC_SHIFT,
+	},
+	{
+		.dt_node_name	= "pwr_sw1",
+		.voltage_table	= fixed_5v_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(fixed_5v_voltage_table),
+		.control_reg	= USB_CONTROL_REG,
+		.enable_mask	= USBSW_OTG_SWITCH_ENABLED,
+		.icc_reg	= BUCK_ICC_TURNOFF_REG,
+		.icc_mask	= PWR_SW1_ICC_SHIFT,
+	},
+	{
+		.dt_node_name	= "pwr_sw2",
+		.voltage_table	= fixed_5v_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(fixed_5v_voltage_table),
+		.control_reg	= USB_CONTROL_REG,
+		.enable_mask	= SWIN_SWOUT_ENABLED,
+		.icc_reg	= BUCK_ICC_TURNOFF_REG,
+		.icc_mask	= PWR_SW2_ICC_SHIFT,
+	},
 };
 
 #define MAX_REGUL	ARRAY_SIZE(regulators_table)
@@ -606,7 +667,7 @@
 				       regul->enable_mask);
 }
 
-uint8_t stpmic1_is_regulator_enabled(const char *name)
+bool stpmic1_is_regulator_enabled(const char *name)
 {
 	uint8_t val;
 	const struct regul_struct *regul = get_regulator_data(name);
@@ -615,7 +676,7 @@
 		panic();
 	}
 
-	return (val & regul->enable_mask);
+	return (val & regul->enable_mask) == regul->enable_mask;
 }
 
 int stpmic1_regulator_voltage_set(const char *name, uint16_t millivolts)
@@ -624,11 +685,21 @@
 	const struct regul_struct *regul = get_regulator_data(name);
 	uint8_t mask;
 
+	if ((strncmp(name, "ldo3", 5) == 0) && ldo3_special_mode) {
+		/*
+		 * when the LDO3 is in special mode, we do not change voltage,
+		 * because by setting voltage, the LDO would leaves sink-source
+		 * mode. There is obviously no reason to leave sink-source mode
+		 * at runtime.
+		 */
+		return 0;
+	}
+
 	/* Voltage can be set for buck<N> or ldo<N> (except ldo4) regulators */
 	if (strncmp(name, "buck", 4) == 0) {
 		mask = BUCK_VOLTAGE_MASK;
 	} else if ((strncmp(name, "ldo", 3) == 0) &&
-		   (strncmp(name, "ldo4", 4) != 0)) {
+		   (strncmp(name, "ldo4", 5) != 0)) {
 		mask = LDO_VOLTAGE_MASK;
 	} else {
 		return 0;
@@ -657,12 +728,90 @@
 {
 	const struct regul_struct *regul = get_regulator_data(name);
 
+	if (regul->mask_reset_reg == 0U) {
+		return -EPERM;
+	}
+
 	return stpmic1_register_update(regul->mask_reset_reg,
 				       BIT(regul->mask_reset),
 				       LDO_BUCK_RESET_MASK <<
 				       regul->mask_reset);
 }
 
+int stpmic1_regulator_icc_set(const char *name)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	if (regul->mask_reset_reg == 0U) {
+		return -EPERM;
+	}
+
+	return stpmic1_register_update(regul->icc_reg,
+				       BIT(regul->icc_mask),
+				       BIT(regul->icc_mask));
+}
+
+int stpmic1_regulator_sink_mode_set(const char *name)
+{
+	if (strncmp(name, "ldo3", 5) != 0) {
+		return -EPERM;
+	}
+
+	ldo3_special_mode = true;
+
+	/* disable bypass mode, enable sink mode */
+	return stpmic1_register_update(LDO3_CONTROL_REG,
+				       LDO3_DDR_SEL << LDO_BUCK_VOLTAGE_SHIFT,
+				       LDO3_BYPASS | LDO_VOLTAGE_MASK);
+}
+
+int stpmic1_regulator_bypass_mode_set(const char *name)
+{
+	if (strncmp(name, "ldo3", 5) != 0) {
+		return -EPERM;
+	}
+
+	ldo3_special_mode = true;
+
+	/* enable bypass mode, disable sink mode */
+	return stpmic1_register_update(LDO3_CONTROL_REG,
+				       LDO3_BYPASS,
+				       LDO3_BYPASS | LDO_VOLTAGE_MASK);
+}
+
+int stpmic1_active_discharge_mode_set(const char *name)
+{
+	if (strncmp(name, "pwr_sw1", 8) == 0) {
+		return stpmic1_register_update(USB_CONTROL_REG,
+					       VBUS_OTG_DISCHARGE,
+					       VBUS_OTG_DISCHARGE);
+	}
+
+	if (strncmp(name, "pwr_sw2", 8) == 0) {
+		return stpmic1_register_update(USB_CONTROL_REG,
+					       SW_OUT_DISCHARGE,
+					       SW_OUT_DISCHARGE);
+	}
+
+	return -EPERM;
+}
+
+int stpmic1_regulator_levels_mv(const char *name, const uint16_t **levels,
+				size_t *levels_count)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	if ((strncmp(name, "ldo3", 5) == 0) && ldo3_special_mode) {
+		*levels_count = ARRAY_SIZE(ldo3_special_mode_table);
+		*levels = ldo3_special_mode_table;
+	} else {
+		*levels_count = regul->voltage_table_size;
+		*levels = regul->voltage_table;
+	}
+
+	return 0;
+}
+
 int stpmic1_regulator_voltage_get(const char *name)
 {
 	const struct regul_struct *regul = get_regulator_data(name);
@@ -670,11 +819,15 @@
 	uint8_t mask;
 	int status;
 
+	if ((strncmp(name, "ldo3", 5) == 0) && ldo3_special_mode) {
+		return 0;
+	}
+
 	/* Voltage can be set for buck<N> or ldo<N> (except ldo4) regulators */
 	if (strncmp(name, "buck", 4) == 0) {
 		mask = BUCK_VOLTAGE_MASK;
 	} else if ((strncmp(name, "ldo", 3) == 0) &&
-		   (strncmp(name, "ldo4", 4) != 0)) {
+		   (strncmp(name, "ldo4", 5) != 0)) {
 		mask = LDO_VOLTAGE_MASK;
 	} else {
 		return 0;
diff --git a/drivers/st/regulator/regulator_core.c b/drivers/st/regulator/regulator_core.c
new file mode 100644
index 0000000..94b3cef
--- /dev/null
+++ b/drivers/st/regulator/regulator_core.c
@@ -0,0 +1,536 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <limits.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/regulator.h>
+#include <libfdt.h>
+
+#define MAX_PROPERTY_LEN 64
+
+static struct rdev rdev_array[PLAT_NB_RDEVS];
+
+#define for_each_rdev(rdev) \
+	for (rdev = rdev_array; rdev < (rdev_array + PLAT_NB_RDEVS); rdev++)
+
+#define for_each_registered_rdev(rdev) \
+	for (rdev = rdev_array; \
+	     (rdev < (rdev_array + PLAT_NB_RDEVS)) && (rdev->desc != NULL); rdev++)
+
+static void lock_driver(const struct rdev *rdev)
+{
+	if (rdev->desc->ops->lock != NULL) {
+		rdev->desc->ops->lock(rdev->desc);
+	}
+}
+
+static void unlock_driver(const struct rdev *rdev)
+{
+	if (rdev->desc->ops->unlock != NULL) {
+		rdev->desc->ops->unlock(rdev->desc);
+	}
+}
+
+static struct rdev *regulator_get_by_phandle(int32_t phandle)
+{
+	struct rdev *rdev;
+
+	for_each_registered_rdev(rdev) {
+		if (rdev->phandle == phandle) {
+			return rdev;
+		}
+	}
+
+	WARN("%s: phandle %d not found\n", __func__, phandle);
+	return NULL;
+}
+
+/*
+ * Get a regulator from its node name
+ *
+ * @fdt - pointer to device tree memory
+ * @node_name - name of the node "ldo1"
+ * Return pointer to rdev if succeed, NULL else.
+ */
+struct rdev *regulator_get_by_name(const char *node_name)
+{
+	struct rdev *rdev;
+
+	assert(node_name != NULL);
+	VERBOSE("get %s\n", node_name);
+
+	for_each_registered_rdev(rdev) {
+		if (strcmp(rdev->desc->node_name, node_name) == 0) {
+			return rdev;
+		}
+	}
+
+	WARN("%s: %s not found\n", __func__, node_name);
+	return NULL;
+}
+
+static int32_t get_supply_phandle(const void *fdt, int node, const char *name)
+{
+	const fdt32_t *cuint;
+	int len __unused;
+	int supply_phandle = -FDT_ERR_NOTFOUND;
+	char prop_name[MAX_PROPERTY_LEN];
+
+	len = snprintf(prop_name, MAX_PROPERTY_LEN - 1, "%s-supply", name);
+	assert((len >= 0) && (len < MAX_PROPERTY_LEN - 1));
+
+	cuint = fdt_getprop(fdt, node, prop_name, NULL);
+	if (cuint != NULL) {
+		supply_phandle = fdt32_to_cpu(*cuint);
+		VERBOSE("%s: supplied by %d\n", name, supply_phandle);
+	}
+
+	return supply_phandle;
+}
+
+/*
+ * Get a regulator from a supply name
+ *
+ * @fdt - pointer to device tree memory
+ * @node - offset of the node that contains the supply description
+ * @name - name of the supply "vdd" for "vdd-supply'
+ * Return pointer to rdev if succeed, NULL else.
+ */
+struct rdev *regulator_get_by_supply_name(const void *fdt, int node, const char *name)
+{
+	const int p = get_supply_phandle(fdt, node, name);
+
+	if (p < 0) {
+		return NULL;
+	}
+
+	return regulator_get_by_phandle(p);
+}
+
+static int __regulator_set_state(struct rdev *rdev, bool state)
+{
+	if (rdev->desc->ops->set_state == NULL) {
+		return -ENODEV;
+	}
+
+	return rdev->desc->ops->set_state(rdev->desc, state);
+}
+
+/*
+ * Enable regulator
+ *
+ * @rdev - pointer to rdev struct
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_enable(struct rdev *rdev)
+{
+	int ret;
+
+	assert(rdev != NULL);
+
+	ret = __regulator_set_state(rdev, STATE_ENABLE);
+
+	udelay(rdev->enable_ramp_delay);
+
+	return ret;
+}
+
+/*
+ * Disable regulator
+ *
+ * @rdev - pointer to rdev struct
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_disable(struct rdev *rdev)
+{
+	int ret;
+
+	assert(rdev != NULL);
+
+	ret = __regulator_set_state(rdev, STATE_DISABLE);
+
+	udelay(rdev->enable_ramp_delay);
+
+	return ret;
+}
+
+/*
+ * Regulator enabled query
+ *
+ * @rdev - pointer to rdev struct
+ * Return 0 if disabled, 1 if enabled, <0 else.
+ */
+int regulator_is_enabled(const struct rdev *rdev)
+{
+	int ret;
+
+	assert(rdev != NULL);
+
+	VERBOSE("%s: is en\n", rdev->desc->node_name);
+
+	if (rdev->desc->ops->get_state == NULL) {
+		return -ENODEV;
+	}
+
+	lock_driver(rdev);
+
+	ret = rdev->desc->ops->get_state(rdev->desc);
+	if (ret < 0) {
+		ERROR("regul %s get state failed: err:%d\n",
+		      rdev->desc->node_name, ret);
+	}
+
+	unlock_driver(rdev);
+
+	return ret;
+}
+
+/*
+ * Set regulator voltage
+ *
+ * @rdev - pointer to rdev struct
+ * @mvolt - Target voltage level in millivolt
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_set_voltage(struct rdev *rdev, uint16_t mvolt)
+{
+	int ret;
+
+	assert(rdev != NULL);
+
+	VERBOSE("%s: set mvolt\n", rdev->desc->node_name);
+
+	if (rdev->desc->ops->set_voltage == NULL) {
+		return -ENODEV;
+	}
+
+	if ((mvolt < rdev->min_mv) || (mvolt > rdev->max_mv)) {
+		return -EPERM;
+	}
+
+	lock_driver(rdev);
+
+	ret = rdev->desc->ops->set_voltage(rdev->desc, mvolt);
+	if (ret < 0) {
+		ERROR("regul %s set volt failed: err:%d\n",
+		      rdev->desc->node_name, ret);
+	}
+
+	unlock_driver(rdev);
+
+	return ret;
+}
+
+/*
+ * Set regulator min voltage
+ *
+ * @rdev - pointer to rdev struct
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_set_min_voltage(struct rdev *rdev)
+{
+	return regulator_set_voltage(rdev, rdev->min_mv);
+}
+
+/*
+ * Get regulator voltage
+ *
+ * @rdev - pointer to rdev struct
+ * Return milli volts if succeed, <0 else.
+ */
+int regulator_get_voltage(const struct rdev *rdev)
+{
+	int ret;
+
+	assert(rdev != NULL);
+
+	VERBOSE("%s: get volt\n", rdev->desc->node_name);
+
+	if (rdev->desc->ops->get_voltage == NULL) {
+		return rdev->min_mv;
+	}
+
+	lock_driver(rdev);
+
+	ret = rdev->desc->ops->get_voltage(rdev->desc);
+	if (ret < 0) {
+		ERROR("regul %s get voltage failed: err:%d\n",
+		      rdev->desc->node_name, ret);
+	}
+
+	unlock_driver(rdev);
+
+	return ret;
+}
+
+/*
+ * List regulator voltages
+ *
+ * @rdev - pointer to rdev struct
+ * @levels - out: array of supported millitvolt levels from min to max value
+ * @count - out: number of possible millivolt values
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_list_voltages(const struct rdev *rdev, const uint16_t **levels, size_t *count)
+{
+	int ret;
+	size_t n;
+
+	assert(rdev != NULL);
+	assert(levels != NULL);
+	assert(count != NULL);
+
+	VERBOSE("%s: list volt\n", rdev->desc->node_name);
+
+	if (rdev->desc->ops->list_voltages == NULL) {
+		return -ENODEV;
+	}
+
+	lock_driver(rdev);
+
+	ret = rdev->desc->ops->list_voltages(rdev->desc, levels, count);
+
+	unlock_driver(rdev);
+
+	if (ret < 0) {
+		ERROR("regul %s list_voltages failed: err: %d\n",
+		      rdev->desc->node_name, ret);
+		return ret;
+	}
+
+	/*
+	 * Reduce the possible values depending on min and max from device-tree
+	 */
+	n = *count;
+	while ((n > 1U) && ((*levels)[n - 1U] > rdev->max_mv)) {
+		n--;
+	}
+
+	/* Verify that max val is a valid value */
+	if (rdev->max_mv != (*levels)[n - 1]) {
+		ERROR("regul %s: max value %u is invalid\n",
+		      rdev->desc->node_name, rdev->max_mv);
+		return -EINVAL;
+	}
+
+	while ((n > 1U) && ((*levels[0U]) < rdev->min_mv)) {
+		(*levels)++;
+		n--;
+	}
+
+	/* Verify that min is not too high */
+	if (n == 0U) {
+		ERROR("regul %s set min voltage is too high\n",
+		      rdev->desc->node_name);
+		return -EINVAL;
+	}
+
+	/* Verify that min val is a valid vlue */
+	if (rdev->min_mv != (*levels)[0U]) {
+		ERROR("regul %s: min value %u is invalid\n",
+		      rdev->desc->node_name, rdev->min_mv);
+		return -EINVAL;
+	}
+
+	*count = n;
+
+	VERBOSE("rdev->min_mv=%u rdev->max_mv=%u\n", rdev->min_mv, rdev->max_mv);
+
+	return 0;
+}
+
+/*
+ * Get regulator voltages range
+ *
+ * @rdev - pointer to rdev struct
+ * @min_mv - out: min possible millivolt value
+ * @max_mv - out: max possible millivolt value
+ * Return 0 if succeed, non 0 else.
+ */
+void regulator_get_range(const struct rdev *rdev, uint16_t *min_mv, uint16_t *max_mv)
+{
+	assert(rdev != NULL);
+
+	if (min_mv != NULL) {
+		*min_mv = rdev->min_mv;
+	}
+	if (max_mv != NULL) {
+		*max_mv = rdev->max_mv;
+	}
+}
+
+/*
+ * Set regulator flag
+ *
+ * @rdev - pointer to rdev struct
+ * @flag - flag value to set (eg: REGUL_OCP)
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_set_flag(struct rdev *rdev, uint16_t flag)
+{
+	int ret;
+
+	/* check that only one bit is set on flag */
+	if (__builtin_popcount(flag) != 1) {
+		return -EINVAL;
+	}
+
+	/* REGUL_ALWAYS_ON and REGUL_BOOT_ON are internal properties of the core */
+	if ((flag == REGUL_ALWAYS_ON) || (flag == REGUL_BOOT_ON)) {
+		rdev->flags |= flag;
+		return 0;
+	}
+
+	if (rdev->desc->ops->set_flag == NULL) {
+		ERROR("%s can not set any flag\n", rdev->desc->node_name);
+		return -ENODEV;
+	}
+
+	lock_driver(rdev);
+
+	ret = rdev->desc->ops->set_flag(rdev->desc, flag);
+
+	unlock_driver(rdev);
+
+	if (ret != 0) {
+		ERROR("%s: could not set flag %d ret=%d\n",
+		      rdev->desc->node_name, flag, ret);
+		return ret;
+	}
+
+	rdev->flags |= flag;
+
+	return 0;
+}
+
+/*
+ * Parse the device-tree for a regulator
+ *
+ * Read min/max voltage from dt and check its validity
+ * Read the properties, and call the driver to set flags
+ * Read power supply phandle
+ * Read and store low power mode states
+ *
+ * @rdev - pointer to rdev struct
+ * @node - device-tree node offset of the regulator
+ * Return 0 if disabled, 1 if enabled, <0 else.
+ */
+static int parse_dt(struct rdev *rdev, int node)
+{
+	void *fdt;
+	const fdt32_t *cuint;
+	const uint16_t *levels;
+	size_t size;
+	int ret;
+
+	VERBOSE("%s: parse dt\n", rdev->desc->node_name);
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	rdev->phandle = fdt_get_phandle(fdt, node);
+
+	cuint = fdt_getprop(fdt, node, "regulator-min-microvolt", NULL);
+	if (cuint != NULL) {
+		uint16_t min_mv;
+
+		min_mv = (uint16_t)(fdt32_to_cpu(*cuint) / 1000U);
+		VERBOSE("%s: min_mv=%d\n", rdev->desc->node_name, (int)min_mv);
+		if (min_mv <= rdev->max_mv) {
+			rdev->min_mv = min_mv;
+		} else {
+			ERROR("%s: min_mv=%d is too high\n",
+			      rdev->desc->node_name, (int)min_mv);
+			return -EINVAL;
+		}
+	}
+
+	cuint = fdt_getprop(fdt, node, "regulator-max-microvolt", NULL);
+	if (cuint != NULL) {
+		uint16_t max_mv;
+
+		max_mv = (uint16_t)(fdt32_to_cpu(*cuint) / 1000U);
+		VERBOSE("%s: max_mv=%d\n", rdev->desc->node_name, (int)max_mv);
+		if (max_mv >= rdev->min_mv) {
+			rdev->max_mv = max_mv;
+		} else {
+			ERROR("%s: max_mv=%d is too low\n",
+			      rdev->desc->node_name, (int)max_mv);
+			return -EINVAL;
+		}
+	}
+
+	/* validate that min and max values can be used */
+	ret = regulator_list_voltages(rdev, &levels, &size);
+	if ((ret != 0) && (ret != -ENODEV)) {
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * Register a regulator driver in regulator framework.
+ * Initialize voltage range from driver description
+ *
+ * @desc - pointer to the regulator description
+ * @node - device-tree node offset of the regulator
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_register(const struct regul_description *desc, int node)
+{
+	struct rdev *rdev;
+
+	assert(desc != NULL);
+
+	VERBOSE("register %s\n", desc->node_name);
+
+	for_each_rdev(rdev) {
+		if (rdev->desc == NULL) {
+			break;
+		}
+	}
+
+	if (rdev == rdev_array + PLAT_NB_RDEVS) {
+		WARN("Not enough place for regulators, PLAT_NB_RDEVS should be increased.\n");
+		return -ENOMEM;
+	}
+
+	rdev->desc = desc;
+	rdev->enable_ramp_delay = rdev->desc->enable_ramp_delay;
+
+	if (rdev->desc->ops->list_voltages != NULL) {
+		int ret;
+		const uint16_t *levels;
+		size_t count;
+
+		lock_driver(rdev);
+
+		ret = rdev->desc->ops->list_voltages(rdev->desc, &levels, &count);
+
+		unlock_driver(rdev);
+
+		if (ret < 0) {
+			ERROR("regul %s set state failed: err:%d\n",
+			      rdev->desc->node_name, ret);
+			return ret;
+		}
+
+		rdev->min_mv = levels[0];
+		rdev->max_mv = levels[count - 1U];
+	} else {
+		rdev->max_mv = UINT16_MAX;
+	}
+
+	return parse_dt(rdev, node);
+}
diff --git a/drivers/st/regulator/regulator_fixed.c b/drivers/st/regulator/regulator_fixed.c
new file mode 100644
index 0000000..f1c224e
--- /dev/null
+++ b/drivers/st/regulator/regulator_fixed.c
@@ -0,0 +1,87 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/st/regulator.h>
+#include <drivers/st/regulator_fixed.h>
+#include <libfdt.h>
+
+#ifndef PLAT_NB_FIXED_REGS
+#error "Missing PLAT_NB_FIXED_REGS"
+#endif
+
+#define FIXED_NAME_LEN 32
+
+struct fixed_data {
+	char name[FIXED_NAME_LEN];
+	uint16_t volt;
+	struct regul_description desc;
+};
+
+static struct fixed_data data[PLAT_NB_FIXED_REGS];
+
+static int fixed_set_state(const struct regul_description *desc, bool state)
+{
+	return 0;
+}
+
+static int fixed_get_state(const struct regul_description *desc)
+{
+	return 1;
+}
+
+static struct regul_ops fixed_ops = {
+	.set_state = fixed_set_state,
+	.get_state = fixed_get_state,
+};
+
+int fixed_regulator_register(void)
+{
+	uint32_t count = 0;
+	void *fdt;
+	int node;
+
+	VERBOSE("fixed reg init!\n");
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	fdt_for_each_compatible_node(fdt, node, "regulator-fixed") {
+		int len __unused;
+		int ret;
+		struct fixed_data *d = &data[count];
+		const char *reg_name;
+
+		reg_name = fdt_get_name(fdt, node, NULL);
+
+		VERBOSE("register fixed reg %s!\n", reg_name);
+
+		len = snprintf(d->name, FIXED_NAME_LEN - 1, "%s", reg_name);
+		assert((len > 0) && (len < (FIXED_NAME_LEN - 1)));
+
+		d->desc.node_name = d->name;
+		d->desc.driver_data = d;
+		d->desc.ops = &fixed_ops;
+
+		ret = regulator_register(&d->desc, node);
+		if (ret != 0) {
+			WARN("%s:%d failed to register %s\n", __func__,
+			     __LINE__, reg_name);
+			return ret;
+		}
+
+		count++;
+		assert(count <= PLAT_NB_FIXED_REGS);
+
+	}
+
+	return 0;
+}
diff --git a/drivers/st/spi/stm32_qspi.c b/drivers/st/spi/stm32_qspi.c
index 4b1a029..d3c26d9 100644
--- a/drivers/st/spi/stm32_qspi.c
+++ b/drivers/st/spi/stm32_qspi.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2019-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
  */
@@ -11,6 +11,7 @@
 
 #include <common/debug.h>
 #include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/spi_mem.h>
 #include <drivers/st/stm32_gpio.h>
@@ -364,7 +365,7 @@
 
 static int stm32_qspi_set_speed(unsigned int hz)
 {
-	unsigned long qspi_clk = stm32mp_clk_get_rate(stm32_qspi.clock_id);
+	unsigned long qspi_clk = clk_get_rate(stm32_qspi.clock_id);
 	uint32_t prescaler = UINT8_MAX;
 	uint32_t csht;
 	int ret;
@@ -494,7 +495,7 @@
 	stm32_qspi.clock_id = (unsigned long)info.clock;
 	stm32_qspi.reset_id = (unsigned int)info.reset;
 
-	stm32mp_clk_enable(stm32_qspi.clock_id);
+	clk_enable(stm32_qspi.clock_id);
 
 	ret = stm32mp_reset_assert(stm32_qspi.reset_id, TIMEOUT_US_1_MS);
 	if (ret != 0) {
diff --git a/fdts/stm32mp157c-ed1.dts b/fdts/stm32mp157c-ed1.dts
index 11e0a61..5c9818f 100644
--- a/fdts/stm32mp157c-ed1.dts
+++ b/fdts/stm32mp157c-ed1.dts
@@ -135,14 +135,15 @@
 
 			vtt_ddr: ldo3 {
 				regulator-name = "vtt_ddr";
-				regulator-min-microvolt = <500000>;
-				regulator-max-microvolt = <750000>;
 				regulator-always-on;
 				regulator-over-current-protection;
+				st,regulator-sink-source;
 			};
 
 			vdd_usb: ldo4 {
 				regulator-name = "vdd_usb";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
 			};
 
 			vdd_sd: ldo5 {
diff --git a/fdts/stm32mp15xx-dkx.dtsi b/fdts/stm32mp15xx-dkx.dtsi
index 9cc5368..975d749 100644
--- a/fdts/stm32mp15xx-dkx.dtsi
+++ b/fdts/stm32mp15xx-dkx.dtsi
@@ -131,10 +131,9 @@
 
 			vtt_ddr: ldo3 {
 				regulator-name = "vtt_ddr";
-				regulator-min-microvolt = <500000>;
-				regulator-max-microvolt = <750000>;
 				regulator-always-on;
 				regulator-over-current-protection;
+				st,regulator-sink-source;
 			};
 
 			vdd_usb: ldo4 {
@@ -160,7 +159,6 @@
 			vref_ddr: vref_ddr {
 				regulator-name = "vref_ddr";
 				regulator-always-on;
-				regulator-over-current-protection;
 			};
 
 			bst_out: boost {
diff --git a/include/arch/aarch32/arch.h b/include/arch/aarch32/arch.h
index a1bd942..bdff25b 100644
--- a/include/arch/aarch32/arch.h
+++ b/include/arch/aarch32/arch.h
@@ -122,6 +122,10 @@
 #define ID_MMFR4_CNP_LENGTH	U(4)
 #define ID_MMFR4_CNP_MASK	U(0xf)
 
+#define ID_MMFR4_CCIDX_SHIFT	U(24)
+#define ID_MMFR4_CCIDX_LENGTH	U(4)
+#define ID_MMFR4_CCIDX_MASK	U(0xf)
+
 /* ID_PFR0 definitions */
 #define ID_PFR0_AMU_SHIFT	U(20)
 #define ID_PFR0_AMU_LENGTH	U(4)
@@ -174,7 +178,7 @@
 #define SCTLR_AFE_BIT		(U(1) << 29)
 #define SCTLR_TE_BIT		(U(1) << 30)
 #define SCTLR_DSSBS_BIT		(U(1) << 31)
-#define SCTLR_RESET_VAL         (SCTLR_RES1 | SCTLR_NTWE_BIT |		\
+#define SCTLR_RESET_VAL		(SCTLR_RES1 | SCTLR_NTWE_BIT |		\
 				SCTLR_NTWI_BIT | SCTLR_CP15BEN_BIT)
 
 /* SDCR definitions */
@@ -295,7 +299,7 @@
 #define CPACR_CP10_SHIFT	U(20)
 #define CPACR_ENABLE_FP_ACCESS	((U(0x3) << CPACR_CP11_SHIFT) |\
 				 (U(0x3) << CPACR_CP10_SHIFT))
-#define CPACR_RESET_VAL         U(0x0)
+#define CPACR_RESET_VAL		U(0x0)
 
 /* FPEXC definitions */
 #define FPEXC_RES1		((U(1) << 10) | (U(1) << 9) | (U(1) << 8))
@@ -495,13 +499,13 @@
 #define CNTP_CTL		U(0x2c)
 
 /* Physical timer control register bit fields shifts and masks */
-#define CNTP_CTL_ENABLE_SHIFT   0
-#define CNTP_CTL_IMASK_SHIFT    1
-#define CNTP_CTL_ISTATUS_SHIFT  2
+#define CNTP_CTL_ENABLE_SHIFT	0
+#define CNTP_CTL_IMASK_SHIFT	1
+#define CNTP_CTL_ISTATUS_SHIFT	2
 
-#define CNTP_CTL_ENABLE_MASK    U(1)
-#define CNTP_CTL_IMASK_MASK     U(1)
-#define CNTP_CTL_ISTATUS_MASK   U(1)
+#define CNTP_CTL_ENABLE_MASK	U(1)
+#define CNTP_CTL_IMASK_MASK	U(1)
+#define CNTP_CTL_ISTATUS_MASK	U(1)
 
 /* MAIR macros */
 #define MAIR0_ATTR_SET(attr, index)	((attr) << ((index) << U(3)))
@@ -559,6 +563,7 @@
 #define CLIDR		p15, 1, c0, c0, 1
 #define CSSELR		p15, 2, c0, c0, 0
 #define CCSIDR		p15, 1, c0, c0, 0
+#define CCSIDR2		p15, 1, c0, c0, 2
 #define HTCR		p15, 4, c2, c0, 2
 #define HMAIR0		p15, 4, c10, c2, 0
 #define ATS1CPR		p15, 0, c7, c8, 0
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index 0fb4e74..29da33c 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -297,6 +297,10 @@
 #define ID_AA64MMFR2_EL1_ST_SHIFT	U(28)
 #define ID_AA64MMFR2_EL1_ST_MASK	ULL(0xf)
 
+#define ID_AA64MMFR2_EL1_CCIDX_SHIFT	U(20)
+#define ID_AA64MMFR2_EL1_CCIDX_MASK	ULL(0xf)
+#define ID_AA64MMFR2_EL1_CCIDX_LENGTH	U(4)
+
 #define ID_AA64MMFR2_EL1_CNP_SHIFT	U(0)
 #define ID_AA64MMFR2_EL1_CNP_MASK	ULL(0xf)
 
diff --git a/include/drivers/allwinner/axp.h b/include/drivers/allwinner/axp.h
index 222820b..8b90c7f 100644
--- a/include/drivers/allwinner/axp.h
+++ b/include/drivers/allwinner/axp.h
@@ -47,6 +47,13 @@
 
 int axp_check_id(void);
 void axp_power_off(void);
+
+#if SUNXI_SETUP_REGULATORS == 1
 void axp_setup_regulators(const void *fdt);
+#else
+static inline void axp_setup_regulators(const void *fdt)
+{
+}
+#endif
 
 #endif /* AXP_H */
diff --git a/include/drivers/clk.h b/include/drivers/clk.h
new file mode 100644
index 0000000..a18f41f
--- /dev/null
+++ b/include/drivers/clk.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CLK_H
+#define CLK_H
+
+#include <stdbool.h>
+
+struct clk_ops {
+	int (*enable)(unsigned long id);
+	void (*disable)(unsigned long id);
+	unsigned long (*get_rate)(unsigned long id);
+	int (*get_parent)(unsigned long id);
+	bool (*is_enabled)(unsigned long id);
+};
+
+int clk_enable(unsigned long id);
+void clk_disable(unsigned long id);
+unsigned long clk_get_rate(unsigned long id);
+bool clk_is_enabled(unsigned long id);
+int clk_get_parent(unsigned long id);
+
+void clk_register(const struct clk_ops *ops);
+
+#endif /* CLK_H */
diff --git a/include/drivers/gpio.h b/include/drivers/gpio.h
index 99c18a4..9bba993 100644
--- a/include/drivers/gpio.h
+++ b/include/drivers/gpio.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,6 +18,7 @@
 #define GPIO_PULL_NONE		ARM_TF_GPIO_PULL_NONE
 #define GPIO_PULL_UP		ARM_TF_GPIO_PULL_UP
 #define GPIO_PULL_DOWN		ARM_TF_GPIO_PULL_DOWN
+#define GPIO_PULL_REPEATER	ARM_TF_GPIO_PULL_REPEATER
 
 typedef struct gpio_ops {
 	int (*get_direction)(int gpio);
diff --git a/include/drivers/st/regulator.h b/include/drivers/st/regulator.h
new file mode 100644
index 0000000..bf583e2
--- /dev/null
+++ b/include/drivers/st/regulator.h
@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef REGULATOR_H
+#define REGULATOR_H
+
+#include <platform_def.h>
+
+#ifndef PLAT_NB_RDEVS
+#error "Missing PLAT_NB_RDEVS"
+#endif
+
+/*
+ * Consumer interface
+ */
+
+/* regulator-always-on : regulator should never be disabled */
+#define REGUL_ALWAYS_ON		BIT(0)
+/*
+ * regulator-boot-on:
+ * It's expected that this regulator was left on by the bootloader.
+ * The core shouldn't prevent it from being turned off later.
+ * The regulator is needed to exit from suspend so it is turned on during suspend entry.
+ */
+#define REGUL_BOOT_ON		BIT(1)
+/* regulator-over-current-protection: Enable over current protection. */
+#define REGUL_OCP		BIT(2)
+/* regulator-active-discharge: enable active discharge. */
+#define REGUL_ACTIVE_DISCHARGE	BIT(3)
+/* regulator-pull-down: Enable pull down resistor when the regulator is disabled. */
+#define REGUL_PULL_DOWN		BIT(4)
+/*
+ * st,mask-reset: set mask reset for the regulator, meaning that the regulator
+ * setting is maintained during pmic reset.
+ */
+#define REGUL_MASK_RESET	BIT(5)
+/* st,regulator-sink-source: set the regulator in sink source mode */
+#define REGUL_SINK_SOURCE	BIT(6)
+/* st,regulator-bypass: set the regulator in bypass mode */
+#define REGUL_ENABLE_BYPASS	BIT(7)
+
+struct rdev *regulator_get_by_name(const char *node_name);
+
+struct rdev *regulator_get_by_supply_name(const void *fdt, int node, const char *name);
+
+int regulator_enable(struct rdev *rdev);
+int regulator_disable(struct rdev *rdev);
+int regulator_is_enabled(const struct rdev *rdev);
+
+int regulator_set_voltage(struct rdev *rdev, uint16_t volt);
+int regulator_set_min_voltage(struct rdev *rdev);
+int regulator_get_voltage(const struct rdev *rdev);
+
+int regulator_list_voltages(const struct rdev *rdev, const uint16_t **levels, size_t *count);
+void regulator_get_range(const struct rdev *rdev, uint16_t *min_mv, uint16_t *max_mv);
+int regulator_set_flag(struct rdev *rdev, uint16_t flag);
+
+/*
+ * Driver Interface
+ */
+
+/* set_state() arguments */
+#define STATE_DISABLE		false
+#define STATE_ENABLE		true
+
+struct regul_description {
+	const char *node_name;
+	const struct regul_ops *ops;
+	const void *driver_data;
+	const char *supply_name;
+	const uint32_t enable_ramp_delay;
+};
+
+struct regul_ops {
+	int (*set_state)(const struct regul_description *desc, bool state);
+	int (*get_state)(const struct regul_description *desc);
+	int (*set_voltage)(const struct regul_description *desc, uint16_t mv);
+	int (*get_voltage)(const struct regul_description *desc);
+	int (*list_voltages)(const struct regul_description *desc,
+			     const uint16_t **levels, size_t *count);
+	int (*set_flag)(const struct regul_description *desc, uint16_t flag);
+	void (*lock)(const struct regul_description *desc);
+	void (*unlock)(const struct regul_description *desc);
+};
+
+int regulator_register(const struct regul_description *desc, int node);
+
+/*
+ * Internal regulator structure
+ * The structure is internal to the core, and the content should not be used
+ * by a consumer nor a driver.
+ */
+struct rdev {
+	const struct regul_description *desc;
+
+	int32_t phandle;
+
+	uint16_t min_mv;
+	uint16_t max_mv;
+
+	uint16_t flags;
+
+	uint32_t enable_ramp_delay;
+};
+
+#endif /* REGULATOR_H */
diff --git a/include/drivers/st/regulator_fixed.h b/include/drivers/st/regulator_fixed.h
new file mode 100644
index 0000000..b981262
--- /dev/null
+++ b/include/drivers/st/regulator_fixed.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef REGULATOR_FIXED_H
+#define REGULATOR_FIXED_H
+
+int fixed_regulator_register(void);
+
+#endif /* REGULATOR_FIXED_H */
diff --git a/include/drivers/st/stm32_sdmmc2.h b/include/drivers/st/stm32_sdmmc2.h
index 4853208..c83f625 100644
--- a/include/drivers/st/stm32_sdmmc2.h
+++ b/include/drivers/st/stm32_sdmmc2.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,6 +10,7 @@
 #include <stdbool.h>
 
 #include <drivers/mmc.h>
+#include <drivers/st/regulator.h>
 
 struct stm32_sdmmc2_params {
 	uintptr_t		reg_base;
@@ -24,6 +25,7 @@
 	unsigned int		reset_id;
 	unsigned int		max_freq;
 	bool			use_dma;
+	struct rdev		*vmmc_regu;
 };
 
 unsigned long long stm32_sdmmc2_mmc_get_device_size(void);
diff --git a/include/drivers/st/stm32mp_pmic.h b/include/drivers/st/stm32mp_pmic.h
index 984cd60..4dfb038 100644
--- a/include/drivers/st/stm32mp_pmic.h
+++ b/include/drivers/st/stm32mp_pmic.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -20,14 +20,6 @@
 int dt_pmic_status(void);
 
 /*
- * dt_pmic_configure_boot_on_regulators - Configure boot-on and always-on
- * regulators from device tree configuration
- *
- * Returns 0 on success, and negative values on errors
- */
-int dt_pmic_configure_boot_on_regulators(void);
-
-/*
  * initialize_pmic_i2c - Initialize I2C for the PMIC control
  *
  * Returns true if PMIC is available, false if not found, panics on errors
@@ -41,6 +33,14 @@
  */
 void initialize_pmic(void);
 
+#if DEBUG
+void print_pmic_info_and_debug(void);
+#else
+static inline void print_pmic_info_and_debug(void)
+{
+}
+#endif
+
 /*
  * pmic_ddr_power_init - Initialize regulators required for DDR
  *
diff --git a/include/drivers/st/stpmic1.h b/include/drivers/st/stpmic1.h
index dc096cd..2dfc7f8 100644
--- a/include/drivers/st/stpmic1.h
+++ b/include/drivers/st/stpmic1.h
@@ -103,6 +103,22 @@
 #define BUCK4_PULL_DOWN_SHIFT		6
 #define VREF_DDR_PULL_DOWN_SHIFT	4
 
+/* ICC register */
+#define BUCK1_ICC_SHIFT			0
+#define BUCK2_ICC_SHIFT			1
+#define BUCK3_ICC_SHIFT			2
+#define BUCK4_ICC_SHIFT			3
+#define PWR_SW1_ICC_SHIFT		4
+#define PWR_SW2_ICC_SHIFT		5
+#define BOOST_ICC_SHIFT			6
+
+#define LDO1_ICC_SHIFT			0
+#define LDO2_ICC_SHIFT			1
+#define LDO3_ICC_SHIFT			2
+#define LDO4_ICC_SHIFT			3
+#define LDO5_ICC_SHIFT			4
+#define LDO6_ICC_SHIFT			5
+
 /* Buck Mask reset register */
 #define BUCK1_MASK_RESET		0
 #define BUCK2_MASK_RESET		1
@@ -118,6 +134,10 @@
 #define LDO6_MASK_RESET			5
 #define VREF_DDR_MASK_RESET		6
 
+/* LDO3 Special modes */
+#define LDO3_BYPASS                     BIT(7)
+#define LDO3_DDR_SEL                    31U
+
 /* Main PMIC Control Register (MAIN_CONTROL_REG) */
 #define ICC_EVENT_ENABLED		BIT(4)
 #define PWRCTRL_POLARITY_HIGH		BIT(3)
@@ -145,9 +165,12 @@
 /* USB Control Register */
 #define BOOST_OVP_DISABLED		BIT(7)
 #define VBUS_OTG_DETECTION_DISABLED	BIT(6)
+#define SW_OUT_DISCHARGE		BIT(5)
+#define VBUS_OTG_DISCHARGE		BIT(4)
 #define OCP_LIMIT_HIGH			BIT(3)
 #define SWIN_SWOUT_ENABLED		BIT(2)
 #define USBSW_OTG_SWITCH_ENABLED	BIT(1)
+#define BOOST_ENABLED			BIT(0)
 
 int stpmic1_powerctrl_on(void);
 int stpmic1_switch_off(void);
@@ -156,11 +179,17 @@
 int stpmic1_register_update(uint8_t register_id, uint8_t value, uint8_t mask);
 int stpmic1_regulator_enable(const char *name);
 int stpmic1_regulator_disable(const char *name);
-uint8_t stpmic1_is_regulator_enabled(const char *name);
+bool stpmic1_is_regulator_enabled(const char *name);
 int stpmic1_regulator_voltage_set(const char *name, uint16_t millivolts);
+int stpmic1_regulator_levels_mv(const char *name, const uint16_t **levels,
+				size_t *levels_count);
 int stpmic1_regulator_voltage_get(const char *name);
 int stpmic1_regulator_pull_down_set(const char *name);
 int stpmic1_regulator_mask_reset_set(const char *name);
+int stpmic1_regulator_icc_set(const char *name);
+int stpmic1_regulator_sink_mode_set(const char *name);
+int stpmic1_regulator_bypass_mode_set(const char *name);
+int stpmic1_active_discharge_mode_set(const char *name);
 void stpmic1_bind_i2c(struct i2c_handle_s *i2c_handle, uint16_t i2c_addr);
 
 int stpmic1_get_version(unsigned long *version);
diff --git a/include/export/drivers/gpio_exp.h b/include/export/drivers/gpio_exp.h
index a37f190..e4112a9 100644
--- a/include/export/drivers/gpio_exp.h
+++ b/include/export/drivers/gpio_exp.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,5 +18,6 @@
 #define ARM_TF_GPIO_PULL_NONE		0
 #define ARM_TF_GPIO_PULL_UP		1
 #define ARM_TF_GPIO_PULL_DOWN		2
+#define ARM_TF_GPIO_PULL_REPEATER	3
 
 #endif /* ARM_TRUSTED_FIRMWARE_EXPORT_DRIVERS_GPIO_EXP_H */
diff --git a/include/lib/cpus/aarch64/cortex_x2.h b/include/lib/cpus/aarch64/cortex_x2.h
index 9ce1223..bf1b460 100644
--- a/include/lib/cpus/aarch64/cortex_x2.h
+++ b/include/lib/cpus/aarch64/cortex_x2.h
@@ -15,9 +15,23 @@
 #define CORTEX_X2_CPUECTLR_EL1					S3_0_C15_C1_4
 
 /*******************************************************************************
+ * CPU Extended Control register 2 specific definitions
+ ******************************************************************************/
+#define CORTEX_X2_CPUECTLR2_EL1					S3_0_C15_C1_5
+
+#define CORTEX_X2_CPUECTLR2_EL1_PF_MODE_SHIFT			U(11)
+#define CORTEX_X2_CPUECTLR2_EL1_PF_MODE_WIDTH			U(4)
+#define CORTEX_X2_CPUECTLR2_EL1_PF_MODE_CNSRV			ULL(0x9)
+
+/*******************************************************************************
  * CPU Power Control register specific definitions
  ******************************************************************************/
 #define CORTEX_X2_CPUPWRCTLR_EL1				S3_0_C15_C2_7
 #define CORTEX_X2_CPUPWRCTLR_EL1_CORE_PWRDN_BIT			U(1)
 
+/*******************************************************************************
+ * CPU Auxiliary Control Register 5 definitions
+ ******************************************************************************/
+#define CORTEX_X2_CPUACTLR5_EL1					S3_0_C15_C8_0
+
 #endif /* CORTEX_X2_H */
diff --git a/lib/aarch32/cache_helpers.S b/lib/aarch32/cache_helpers.S
index 7cbefe6..13d1872 100644
--- a/lib/aarch32/cache_helpers.S
+++ b/lib/aarch32/cache_helpers.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2021, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -91,6 +91,8 @@
 
 func do_dcsw_op
 	push	{r4-r12, lr}
+	ldcopr	r8, ID_MMFR4		// stash FEAT_CCIDX identifier in r8
+	ubfx	r8, r8, #ID_MMFR4_CCIDX_SHIFT, #ID_MMFR4_CCIDX_LENGTH
 	adr	r11, dcsw_loop_table	// compute cache op based on the operation type
 	add	r6, r11, r0, lsl #3	// cache op is 2x32-bit instructions
 loop1:
@@ -105,13 +107,25 @@
 	ldcopr	r12, CCSIDR		// read the new ccsidr
 	and	r10, r12, #7		// extract the length of the cache lines
 	add	r10, r10, #4		// add 4 (r10 = line length offset)
-	ubfx	r4, r12, #3, #10	// r4 = maximum way number (right aligned)
+
+	cmp	r8, #0			// check for FEAT_CCIDX for Associativity
+	beq	1f
+	ubfx	r4, r12, #3, #21 	// r4 = associativity CCSIDR[23:3]
+	b	2f
+1:
+	ubfx	r4, r12, #3, #10 	// r4 = associativity CCSIDR[12:3]
+2:
 	clz	r5, r4			// r5 = the bit position of the way size increment
 	mov	r9, r4			// r9 working copy of the aligned max way number
 
 loop2:
-	ubfx	r7, r12, #13, #15	// r7 = max set number (right aligned)
-
+	cmp	r8, #0			// check for FEAT_CCIDX for NumSets
+	beq	3f
+	ldcopr	r12, CCSIDR2		// FEAT_CCIDX numsets is in CCSIDR2
+	ubfx	r7, r12, #0, #24	// r7 = numsets CCSIDR2[23:0]
+	b	loop3
+3:
+	ubfx	r7, r12, #13, #15	// r7 = numsets CCSIDR[27:13]
 loop3:
 	orr	r0, r1, r9, LSL r5	// factor in the way number and cache level into r0
 	orr	r0, r0, r7, LSL r10	// factor in the set number
diff --git a/lib/aarch64/cache_helpers.S b/lib/aarch64/cache_helpers.S
index de9c8e4..d1f3847 100644
--- a/lib/aarch64/cache_helpers.S
+++ b/lib/aarch64/cache_helpers.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2021, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -89,6 +89,8 @@
 
 func do_dcsw_op
 	cbz	x3, exit
+	mrs	x12, ID_AA64MMFR2_EL1	// stash FEAT_CCIDX identifier in x12
+	ubfx	x12, x12, #ID_AA64MMFR2_EL1_CCIDX_SHIFT, #ID_AA64MMFR2_EL1_CCIDX_LENGTH
 	adr	x14, dcsw_loop_table	// compute inner loop address
 	add	x14, x14, x0, lsl #5	// inner loop is 8x32-bit instructions
 #if ENABLE_BTI
@@ -108,12 +110,25 @@
 	mrs	x1, ccsidr_el1		// read the new ccsidr
 	and	x2, x1, #7		// extract the length of the cache lines
 	add	x2, x2, #4		// add 4 (line length offset)
-	ubfx	x4, x1, #3, #10		// maximum way number
+
+	cbz	x12, 1f			// check for FEAT_CCIDX for Associativity
+	ubfx	x4, x1, #3, #21 	// x4 = associativity CCSIDR_EL1[23:3]
+	b 	2f
+1:
+	ubfx	x4, x1, #3, #10 	// x4 = associativity CCSIDR_EL1[12:3]
+2:
 	clz	w5, w4			// bit position of way size increment
 	lsl	w9, w4, w5		// w9 = aligned max way number
 	lsl	w16, w8, w5		// w16 = way number loop decrement
 	orr	w9, w10, w9		// w9 = combine way and cache number
-	ubfx	w6, w1, #13, #15	// w6 = max set number
+
+	cbz	x12, 3f			// check for FEAT_CCIDX for NumSets
+	ubfx	x6, x1, #32, #24	// x6 (w6) = numsets CCSIDR_EL1[55:32]
+					// ISA will not allow x->w ubfx
+	b	4f
+3:
+	ubfx	w6, w1, #13, #15	// w6 = numsets CCSIDR_EL1[27:13]
+4:
 	lsl	w17, w8, w2		// w17 = set number loop decrement
 	dsb	sy			// barrier before we start this level
 	br	x14			// jump to DC operation specific loop
diff --git a/lib/cpus/aarch64/cortex_x2.S b/lib/cpus/aarch64/cortex_x2.S
index 87a9bdf..88116c3 100644
--- a/lib/cpus/aarch64/cortex_x2.S
+++ b/lib/cpus/aarch64/cortex_x2.S
@@ -21,6 +21,98 @@
 #error "Cortex X2 supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
 #endif
 
+	/* --------------------------------------------------
+	 * Errata Workaround for Cortex X2 Errata #2002765.
+	 * This applies to revisions r0p0, r1p0, and r2p0 and
+	 * is open.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0, x1, x17
+	 * --------------------------------------------------
+	 */
+func errata_cortex_x2_2002765_wa
+	/* Check workaround compatibility. */
+	mov	x17, x30
+	bl	check_errata_2002765
+	cbz	x0, 1f
+
+	ldr	x0, =0x6
+	msr	S3_6_C15_C8_0, x0 /* CPUPSELR_EL3 */
+	ldr	x0, =0xF3A08002
+	msr	S3_6_C15_C8_2, x0 /* CPUPOR_EL3 */
+	ldr	x0, =0xFFF0F7FE
+	msr	S3_6_C15_C8_3, x0 /* CPUPMR_EL3 */
+	ldr	x0, =0x40000001003ff
+	msr	S3_6_C15_C8_1, x0 /* CPUPCR_EL3 */
+	isb
+
+1:
+	ret	x17
+endfunc errata_cortex_x2_2002765_wa
+
+func check_errata_2002765
+	/* Applies to r0p0 - r2p0 */
+	mov	x1, #0x20
+	b	cpu_rev_var_ls
+endfunc check_errata_2002765
+
+	/* --------------------------------------------------
+	 * Errata Workaround for Cortex X2 Errata #2058056.
+	 * This applies to revisions r0p0, r1p0, and r2p0 and
+	 * is open.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0, x1, x17
+	 * --------------------------------------------------
+	 */
+func errata_cortex_x2_2058056_wa
+	/* Check workaround compatibility. */
+	mov	x17, x30
+	bl	check_errata_2058056
+	cbz	x0, 1f
+
+	mrs	x1, CORTEX_X2_CPUECTLR2_EL1
+	mov	x0, #CORTEX_X2_CPUECTLR2_EL1_PF_MODE_CNSRV
+	bfi	x1, x0, #CORTEX_X2_CPUECTLR2_EL1_PF_MODE_SHIFT, #CORTEX_X2_CPUECTLR2_EL1_PF_MODE_WIDTH
+	msr	CORTEX_X2_CPUECTLR2_EL1, x1
+
+1:
+	ret	x17
+endfunc errata_cortex_x2_2058056_wa
+
+func check_errata_2058056
+	/* Applies to r0p0 - r2p0 */
+	mov	x1, #0x20
+	b	cpu_rev_var_ls
+endfunc check_errata_2058056
+
+	/* --------------------------------------------------
+	 * Errata Workaround for Cortex X2 Errata #2083908.
+	 * This applies to revision r2p0 and is open.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0-x2, x17
+	 * --------------------------------------------------
+	 */
+func errata_cortex_x2_2083908_wa
+	/* Check workaround compatibility. */
+	mov	x17, x30
+	bl	check_errata_2083908
+	cbz	x0, 1f
+
+	/* Apply the workaround by setting bit 13 in CPUACTLR5_EL1. */
+	mrs	x1, CORTEX_X2_CPUACTLR5_EL1
+	orr	x1, x1, #BIT(13)
+	msr	CORTEX_X2_CPUACTLR5_EL1, x1
+
+1:
+	ret	x17
+endfunc errata_cortex_x2_2083908_wa
+
+func check_errata_2083908
+	/* Applies to r2p0 */
+	mov	x1, #0x20
+	mov	x2, #0x20
+	b	cpu_rev_var_range
+endfunc check_errata_2083908
+
 	/* ----------------------------------------------------
 	 * HW will do the cache maintenance while powering down
 	 * ----------------------------------------------------
@@ -42,15 +134,51 @@
 	 */
 #if REPORT_ERRATA
 func cortex_x2_errata_report
+	stp	x8, x30, [sp, #-16]!
+
+	bl	cpu_get_rev_var
+	mov	x8, x0
+
+	/*
+	 * Report all errata. The revision-variant information is passed to
+	 * checking functions of each errata.
+	 */
+	report_errata ERRATA_X2_2002765, cortex_x2, 2002765
+	report_errata ERRATA_X2_2058056, cortex_x2, 2058056
+	report_errata ERRATA_X2_2083908, cortex_x2, 2083908
+
+	ldp	x8, x30, [sp], #16
 	ret
 endfunc cortex_x2_errata_report
 #endif
 
 func cortex_x2_reset_func
+	mov	x19, x30
+
 	/* Disable speculative loads */
 	msr	SSBS, xzr
 	isb
-	ret
+
+	/* Get the CPU revision and stash it in x18. */
+	bl	cpu_get_rev_var
+	mov	x18, x0
+
+#if ERRATA_X2_2002765
+	mov	x0, x18
+	bl	errata_cortex_x2_2002765_wa
+#endif
+
+#if ERRATA_X2_2058056
+	mov	x0, x18
+	bl	errata_cortex_x2_2058056_wa
+#endif
+
+#if ERRATA_X2_2083908
+	mov	x0, x18
+	bl	errata_cortex_x2_2083908_wa
+#endif
+
+	ret x19
 endfunc cortex_x2_reset_func
 
 	/* ---------------------------------------------
diff --git a/lib/cpus/cpu-ops.mk b/lib/cpus/cpu-ops.mk
index a5b8aae..dd06cb8 100644
--- a/lib/cpus/cpu-ops.mk
+++ b/lib/cpus/cpu-ops.mk
@@ -495,6 +495,18 @@
 # to revision r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is still open.
 ERRATA_A710_2017096	?=0
 
+# Flag to apply erratum 2002765 workaround during reset. This erratum applies
+# to revisions r0p0, r1p0, and r2p0 of the Cortex-X2 cpu and is still open.
+ERRATA_X2_2002765   ?=0
+
+# Flag to apply erratum 2058056 workaround during reset. This erratum applies
+# to revisions r0p0, r1p0, and r2p0 of the Cortex-X2 cpu and is still open.
+ERRATA_X2_2058056   ?=0
+
+# Flag to apply erratum 2083908 workaround during reset. This erratum applies
+# to revision r2p0 of the Cortex-X2 cpu and is still open.
+ERRATA_X2_2083908   ?=0
+
 # Flag to apply DSU erratum 798953. This erratum applies to DSUs revision r0p0.
 # Applying the workaround results in higher DSU power consumption on idle.
 ERRATA_DSU_798953	?=0
@@ -920,6 +932,18 @@
 $(eval $(call assert_boolean,ERRATA_A710_2017096))
 $(eval $(call add_define,ERRATA_A710_2017096))
 
+# Process ERRATA_X2_2002765 flag
+$(eval $(call assert_boolean,ERRATA_X2_2002765))
+$(eval $(call add_define,ERRATA_X2_2002765))
+
+# Process ERRATA_X2_2058056 flag
+$(eval $(call assert_boolean,ERRATA_X2_2058056))
+$(eval $(call add_define,ERRATA_X2_2058056))
+
+# Process ERRATA_X2_2083908 flag
+$(eval $(call assert_boolean,ERRATA_X2_2083908))
+$(eval $(call add_define,ERRATA_X2_2083908))
+
 # Process ERRATA_DSU_798953 flag
 $(eval $(call assert_boolean,ERRATA_DSU_798953))
 $(eval $(call add_define,ERRATA_DSU_798953))
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index 72f5e33..551b689 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -327,6 +327,10 @@
 
 # Enable SVE for non-secure world by default
 ENABLE_SVE_FOR_NS		:= 1
+# SVE is only supported on AArch64 so disable it on AArch32.
+ifeq (${ARCH},aarch32)
+	override ENABLE_SVE_FOR_NS	:= 0
+endif
 ENABLE_SVE_FOR_SWD		:= 0
 
 # SME defaults to disabled
diff --git a/plat/allwinner/common/allwinner-common.mk b/plat/allwinner/common/allwinner-common.mk
index 61ae9b6..34fdaf6 100644
--- a/plat/allwinner/common/allwinner-common.mk
+++ b/plat/allwinner/common/allwinner-common.mk
@@ -61,6 +61,10 @@
 				${AW_PLAT}/common/sunxi_scpi_pm.c
 endif
 
+SUNXI_SETUP_REGULATORS	?=	1
+$(eval $(call assert_boolean,SUNXI_SETUP_REGULATORS))
+$(eval $(call add_define,SUNXI_SETUP_REGULATORS))
+
 # The bootloader is guaranteed to only run on CPU 0 by the boot ROM.
 COLD_BOOT_SINGLE_CPU		:=	1
 
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 0d2c319..b7f9c61 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -142,7 +142,8 @@
 					lib/cpus/aarch64/cortex_a65ae.S		\
 					lib/cpus/aarch64/cortex_a78c.S		\
 					lib/cpus/aarch64/cortex_hayes.S		\
-					lib/cpus/aarch64/cortex_hunter.S
+					lib/cpus/aarch64/cortex_hunter.S	\
+					lib/cpus/aarch64/cortex_x2.S
 	endif
 	# AArch64/AArch32 cores
 	FVP_CPU_LIBS	+=	lib/cpus/aarch64/cortex_a55.S		\
diff --git a/plat/mediatek/mt8186/aarch64/platform_common.c b/plat/mediatek/mt8186/aarch64/platform_common.c
index 745e547..9b9697f 100644
--- a/plat/mediatek/mt8186/aarch64/platform_common.c
+++ b/plat/mediatek/mt8186/aarch64/platform_common.c
@@ -13,8 +13,6 @@
 	/* for TF text, RO, RW */
 	MAP_REGION_FLAT(MTK_DEV_RNG0_BASE, MTK_DEV_RNG0_SIZE,
 			MT_DEVICE | MT_RW | MT_SECURE),
-	MAP_REGION_FLAT(MTK_DEV_RNG1_BASE, MTK_DEV_RNG1_SIZE,
-			MT_DEVICE | MT_RW | MT_SECURE),
 	MAP_REGION_FLAT(MTK_DEV_RNG2_BASE, MTK_DEV_RNG2_SIZE,
 			MT_DEVICE | MT_RW | MT_SECURE),
 	{ 0 }
diff --git a/plat/mediatek/mt8186/bl31_plat_setup.c b/plat/mediatek/mt8186/bl31_plat_setup.c
index 3fd1c71..b70245f 100644
--- a/plat/mediatek/mt8186/bl31_plat_setup.c
+++ b/plat/mediatek/mt8186/bl31_plat_setup.c
@@ -11,10 +11,16 @@
 #include <common/bl_common.h>
 #include <common/debug.h>
 #include <common/desc_image_load.h>
+#include <drivers/generic_delay_timer.h>
 #include <drivers/ti/uart/uart_16550.h>
 #include <lib/coreboot.h>
 
 /* Platform Includes */
+#include <emi_mpu.h>
+#include <mt_gic_v3.h>
+#include <mt_timer.h>
+#include <mtgpio.h>
+#include <mtk_dcm.h>
 #include <plat_params.h>
 #include <plat_private.h>
 
@@ -79,6 +85,17 @@
  ******************************************************************************/
 void bl31_platform_setup(void)
 {
+	dcm_set_default();
+
+	/* Initialize the GIC driver, CPU and distributor interfaces */
+	mt_gic_driver_init();
+	mt_gic_init();
+
+	mt_gpio_init();
+	mt_systimer_init();
+	generic_delay_timer_init();
+
+	emi_mpu_init();
 }
 
 /*******************************************************************************
diff --git a/plat/mediatek/mt8186/drivers/dcm/mtk_dcm.c b/plat/mediatek/mt8186/drivers/dcm/mtk_dcm.c
new file mode 100644
index 0000000..5dde5c5
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/dcm/mtk_dcm.c
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <mtk_dcm.h>
+#include <mtk_dcm_utils.h>
+
+static void dcm_armcore(bool mode)
+{
+	dcm_mp_cpusys_top_bus_pll_div_dcm(mode);
+	dcm_mp_cpusys_top_cpu_pll_div_0_dcm(mode);
+	dcm_mp_cpusys_top_cpu_pll_div_1_dcm(mode);
+}
+
+static void dcm_mcusys(bool on)
+{
+	dcm_mp_cpusys_top_adb_dcm(on);
+	dcm_mp_cpusys_top_apb_dcm(on);
+	dcm_mp_cpusys_top_cpubiu_dcm(on);
+	dcm_mp_cpusys_top_cpubiu_dbg_cg(on);
+	dcm_mp_cpusys_top_misc_dcm(on);
+	dcm_mp_cpusys_top_mp0_qdcm(on);
+	dcm_cpccfg_reg_emi_wfifo(on);
+	dcm_mp_cpusys_top_last_cor_idle_dcm(on);
+}
+
+static void dcm_stall(bool on)
+{
+	dcm_mp_cpusys_top_core_stall_dcm(on);
+	dcm_mp_cpusys_top_fcm_stall_dcm(on);
+}
+
+static bool check_dcm_state(void)
+{
+	bool ret = true;
+
+	ret &= dcm_mp_cpusys_top_bus_pll_div_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_cpu_pll_div_0_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_cpu_pll_div_1_dcm_is_on();
+
+	ret &= dcm_mp_cpusys_top_adb_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_apb_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_cpubiu_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_cpubiu_dbg_cg_is_on();
+	ret &= dcm_mp_cpusys_top_misc_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_mp0_qdcm_is_on();
+	ret &= dcm_cpccfg_reg_emi_wfifo_is_on();
+	ret &= dcm_mp_cpusys_top_last_cor_idle_dcm_is_on();
+
+	ret &= dcm_mp_cpusys_top_core_stall_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_fcm_stall_dcm_is_on();
+
+	return ret;
+}
+
+void dcm_set_default(void)
+{
+	dcm_armcore(true);
+	dcm_mcusys(true);
+	dcm_stall(true);
+
+	INFO("%s: %d", __func__, check_dcm_state());
+}
diff --git a/plat/mediatek/mt8186/drivers/dcm/mtk_dcm.h b/plat/mediatek/mt8186/drivers/dcm/mtk_dcm.h
new file mode 100644
index 0000000..6abcff4
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/dcm/mtk_dcm.h
@@ -0,0 +1,14 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTK_DCM_H
+#define MTK_DCM_H
+
+#include <stdbool.h>
+
+void dcm_set_default(void);
+
+#endif /* #ifndef MTK_DCM_H */
diff --git a/plat/mediatek/mt8186/drivers/dcm/mtk_dcm_utils.c b/plat/mediatek/mt8186/drivers/dcm/mtk_dcm_utils.c
new file mode 100644
index 0000000..ae0e964
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/dcm/mtk_dcm_utils.c
@@ -0,0 +1,490 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <mtk_dcm_utils.h>
+
+#define MP_CPUSYS_TOP_ADB_DCM_REG0_MASK (BIT(16) |	\
+					 BIT(17) |	\
+					 BIT(18) |	\
+					 BIT(21))
+#define MP_CPUSYS_TOP_ADB_DCM_REG1_MASK (BIT(16) |	\
+					 BIT(17) |	\
+					 BIT(18))
+#define MP_CPUSYS_TOP_ADB_DCM_REG0_ON (BIT(16) |	\
+				       BIT(17) |	\
+				       BIT(18) |	\
+				       BIT(21))
+#define MP_CPUSYS_TOP_ADB_DCM_REG1_ON (BIT(16) |	\
+				       BIT(17) |	\
+				       BIT(18))
+#define MP_CPUSYS_TOP_ADB_DCM_REG0_OFF ((0x0 << 16) |	\
+					(0x0 << 17) |	\
+					(0x0 << 18) |	\
+					(0x0 << 21))
+#define MP_CPUSYS_TOP_ADB_DCM_REG1_OFF ((0x0 << 16) |	\
+					(0x0 << 17) |	\
+					(0x0 << 18))
+
+bool dcm_mp_cpusys_top_adb_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP_ADB_DCM_CFG4) &
+		MP_CPUSYS_TOP_ADB_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_ADB_DCM_REG0_ON);
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0) &
+		MP_CPUSYS_TOP_ADB_DCM_REG1_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_ADB_DCM_REG1_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_adb_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_adb_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_ADB_DCM_CFG4,
+			MP_CPUSYS_TOP_ADB_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_ADB_DCM_REG0_ON);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+			MP_CPUSYS_TOP_ADB_DCM_REG1_MASK,
+			MP_CPUSYS_TOP_ADB_DCM_REG1_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_adb_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_ADB_DCM_CFG4,
+			MP_CPUSYS_TOP_ADB_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_ADB_DCM_REG0_OFF);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+			MP_CPUSYS_TOP_ADB_DCM_REG1_MASK,
+			MP_CPUSYS_TOP_ADB_DCM_REG1_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_APB_DCM_REG0_MASK (BIT(5))
+#define MP_CPUSYS_TOP_APB_DCM_REG1_MASK (BIT(8))
+#define MP_CPUSYS_TOP_APB_DCM_REG2_MASK (BIT(16))
+#define MP_CPUSYS_TOP_APB_DCM_REG0_ON (BIT(5))
+#define MP_CPUSYS_TOP_APB_DCM_REG1_ON (BIT(8))
+#define MP_CPUSYS_TOP_APB_DCM_REG2_ON (BIT(16))
+#define MP_CPUSYS_TOP_APB_DCM_REG0_OFF ((0x0 << 5))
+#define MP_CPUSYS_TOP_APB_DCM_REG1_OFF ((0x0 << 8))
+#define MP_CPUSYS_TOP_APB_DCM_REG2_OFF ((0x0 << 16))
+
+bool dcm_mp_cpusys_top_apb_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0) &
+		MP_CPUSYS_TOP_APB_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_APB_DCM_REG0_ON);
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0) &
+		MP_CPUSYS_TOP_APB_DCM_REG1_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_APB_DCM_REG1_ON);
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP0_DCM_CFG0) &
+		MP_CPUSYS_TOP_APB_DCM_REG2_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_APB_DCM_REG2_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_apb_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_apb_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG0_ON);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG1_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG1_ON);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG2_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG2_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_apb_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG0_OFF);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG1_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG1_OFF);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG2_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG2_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_MASK (BIT(11) |	\
+						 BIT(24) |	\
+						 BIT(25))
+#define MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_ON (BIT(11) |	\
+					       BIT(24) |	\
+					       BIT(25))
+#define MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_OFF ((0x0 << 11) |	\
+						(0x0 << 24) |	\
+						(0x0 << 25))
+
+bool dcm_mp_cpusys_top_bus_pll_div_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG) &
+		MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_bus_pll_div_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_bus_pll_div_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+			MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_bus_pll_div_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+			MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_MASK (BIT(0))
+#define MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_ON (BIT(0))
+#define MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_OFF ((0x0 << 0))
+
+bool dcm_mp_cpusys_top_core_stall_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP0_DCM_CFG7) &
+		MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_core_stall_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_core_stall_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+			MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_core_stall_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+			MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_MASK (BIT(0))
+#define MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_ON ((0x0 << 0))
+#define MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_OFF (BIT(0))
+
+bool dcm_mp_cpusys_top_cpubiu_dbg_cg_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MCSI_CFG2) &
+		MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_cpubiu_dbg_cg(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_cpubiu_dbg_cg'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCSI_CFG2,
+			MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_MASK,
+			MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_cpubiu_dbg_cg'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCSI_CFG2,
+			MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_MASK,
+			MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CPUBIU_DCM_REG0_MASK ((0xffff << 0))
+#define MP_CPUSYS_TOP_CPUBIU_DCM_REG0_ON ((0xffff << 0))
+#define MP_CPUSYS_TOP_CPUBIU_DCM_REG0_OFF ((0x0 << 0))
+
+bool dcm_mp_cpusys_top_cpubiu_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MCSIC_DCM0) &
+		MP_CPUSYS_TOP_CPUBIU_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_CPUBIU_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_cpubiu_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_cpubiu_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCSIC_DCM0,
+			MP_CPUSYS_TOP_CPUBIU_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPUBIU_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_cpubiu_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCSIC_DCM0,
+			MP_CPUSYS_TOP_CPUBIU_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPUBIU_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_MASK (BIT(11) |	\
+						   BIT(24) |	\
+						   BIT(25))
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_ON (BIT(11) |	\
+						 BIT(24) |	\
+						 BIT(25))
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_OFF ((0x0 << 11) |	\
+						  (0x0 << 24) |	\
+						  (0x0 << 25))
+
+bool dcm_mp_cpusys_top_cpu_pll_div_0_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG0) &
+		MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_cpu_pll_div_0_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_cpu_pll_div_0_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG0,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_cpu_pll_div_0_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG0,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_MASK (BIT(11) |	\
+						   BIT(24) |	\
+						   BIT(25))
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_ON (BIT(11) |	\
+						 BIT(24) |	\
+						 BIT(25))
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_OFF ((0x0 << 11) |	\
+						  (0x0 << 24) |	\
+						  (0x0 << 25))
+
+bool dcm_mp_cpusys_top_cpu_pll_div_1_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG1) &
+		MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_cpu_pll_div_1_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_cpu_pll_div_1_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG1,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_cpu_pll_div_1_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG1,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_MASK (BIT(4))
+#define MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_ON (BIT(4))
+#define MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_OFF ((0x0 << 4))
+
+bool dcm_mp_cpusys_top_fcm_stall_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP0_DCM_CFG7) &
+		MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_fcm_stall_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_fcm_stall_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+			MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_fcm_stall_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+			MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_MASK (BIT(31))
+#define MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_ON (BIT(31))
+#define MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_OFF ((0x0 << 31))
+
+bool dcm_mp_cpusys_top_last_cor_idle_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG) &
+		MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_last_cor_idle_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_last_cor_idle_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+			MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_last_cor_idle_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+			MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_MISC_DCM_REG0_MASK (BIT(0) |	\
+					  BIT(1) |	\
+					  BIT(2) |	\
+					  BIT(3) |	\
+					  BIT(4))
+#define MP_CPUSYS_TOP_MISC_DCM_REG0_ON (BIT(0) |	\
+					BIT(1) |	\
+					BIT(2) |	\
+					BIT(3) |	\
+					BIT(4))
+#define MP_CPUSYS_TOP_MISC_DCM_REG0_OFF ((0x0 << 0) |	\
+					 (0x0 << 1) |	\
+					 (0x0 << 2) |	\
+					 (0x0 << 3) |	\
+					 (0x0 << 4))
+
+bool dcm_mp_cpusys_top_misc_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0) &
+		MP_CPUSYS_TOP_MISC_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_MISC_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_misc_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_misc_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			MP_CPUSYS_TOP_MISC_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_MISC_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_misc_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			MP_CPUSYS_TOP_MISC_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_MISC_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_MP0_QDCM_REG0_MASK (BIT(0) |	\
+			BIT(1) |			\
+			BIT(2) |			\
+			BIT(3))
+#define MP_CPUSYS_TOP_MP0_QDCM_REG0_ON (BIT(0) |	\
+			BIT(1) |			\
+			BIT(2) |			\
+			BIT(3))
+#define MP_CPUSYS_TOP_MP0_QDCM_REG0_OFF ((0x0 << 0) |	\
+			(0x0 << 1) |			\
+			(0x0 << 2) |			\
+			(0x0 << 3))
+
+bool dcm_mp_cpusys_top_mp0_qdcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP0_DCM_CFG0) &
+		MP_CPUSYS_TOP_MP0_QDCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_MP0_QDCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_mp0_qdcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_mp0_qdcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+			MP_CPUSYS_TOP_MP0_QDCM_REG0_MASK,
+			MP_CPUSYS_TOP_MP0_QDCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_mp0_qdcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+			MP_CPUSYS_TOP_MP0_QDCM_REG0_MASK,
+			MP_CPUSYS_TOP_MP0_QDCM_REG0_OFF);
+	}
+}
+
+#define CPCCFG_REG_EMI_WFIFO_REG0_MASK (BIT(0) | BIT(2))
+#define CPCCFG_REG_EMI_WFIFO_REG0_ON (BIT(0) | BIT(2))
+#define CPCCFG_REG_EMI_WFIFO_REG0_OFF ((0x0 << 0) | (0x0 << 2))
+
+bool dcm_cpccfg_reg_emi_wfifo_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(CPCCFG_REG_EMI_WFIFO) &
+		CPCCFG_REG_EMI_WFIFO_REG0_MASK) ==
+		(unsigned int) CPCCFG_REG_EMI_WFIFO_REG0_ON);
+
+	return ret;
+}
+
+void dcm_cpccfg_reg_emi_wfifo(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'cpccfg_reg_emi_wfifo'" */
+		mmio_clrsetbits_32(CPCCFG_REG_EMI_WFIFO,
+			CPCCFG_REG_EMI_WFIFO_REG0_MASK,
+			CPCCFG_REG_EMI_WFIFO_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'cpccfg_reg_emi_wfifo'" */
+		mmio_clrsetbits_32(CPCCFG_REG_EMI_WFIFO,
+			CPCCFG_REG_EMI_WFIFO_REG0_MASK,
+			CPCCFG_REG_EMI_WFIFO_REG0_OFF);
+	}
+}
+
diff --git a/plat/mediatek/mt8186/drivers/dcm/mtk_dcm_utils.h b/plat/mediatek/mt8186/drivers/dcm/mtk_dcm_utils.h
new file mode 100644
index 0000000..ba76594
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/dcm/mtk_dcm_utils.h
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTK_DCM_UTILS_H
+#define MTK_DCM_UTILS_H
+
+#include <stdbool.h>
+
+#include <mtk_dcm.h>
+#include <platform_def.h>
+
+/* Base */
+#define MP_CPUSYS_TOP_BASE	0xc538000
+#define CPCCFG_REG_BASE		0xc53a800
+
+/* Register Definition */
+#define CPCCFG_REG_EMI_WFIFO		(CPCCFG_REG_BASE + 0x100)
+#define MP_CPUSYS_TOP_BUS_PLLDIV_CFG	(MP_CPUSYS_TOP_BASE + 0x22e0)
+#define MP_CPUSYS_TOP_CPU_PLLDIV_CFG0	(MP_CPUSYS_TOP_BASE + 0x22a0)
+#define MP_CPUSYS_TOP_CPU_PLLDIV_CFG1	(MP_CPUSYS_TOP_BASE + 0x22a4)
+#define MP_CPUSYS_TOP_MCSIC_DCM0	(MP_CPUSYS_TOP_BASE + 0x2440)
+#define MP_CPUSYS_TOP_MCSI_CFG2		(MP_CPUSYS_TOP_BASE + 0x2418)
+#define MP_CPUSYS_TOP_MCUSYS_DCM_CFG0	(MP_CPUSYS_TOP_BASE + 0x25c0)
+#define MP_CPUSYS_TOP_MP0_DCM_CFG0	(MP_CPUSYS_TOP_BASE + 0x4880)
+#define MP_CPUSYS_TOP_MP0_DCM_CFG7	(MP_CPUSYS_TOP_BASE + 0x489c)
+#define MP_CPUSYS_TOP_MP_ADB_DCM_CFG4	(MP_CPUSYS_TOP_BASE + 0x2510)
+#define MP_CPUSYS_TOP_MP_MISC_DCM_CFG0	(MP_CPUSYS_TOP_BASE + 0x2518)
+
+bool dcm_mp_cpusys_top_adb_dcm_is_on(void);
+void dcm_mp_cpusys_top_adb_dcm(bool on);
+bool dcm_mp_cpusys_top_apb_dcm_is_on(void);
+void dcm_mp_cpusys_top_apb_dcm(bool on);
+bool dcm_mp_cpusys_top_bus_pll_div_dcm_is_on(void);
+void dcm_mp_cpusys_top_bus_pll_div_dcm(bool on);
+bool dcm_mp_cpusys_top_core_stall_dcm_is_on(void);
+void dcm_mp_cpusys_top_core_stall_dcm(bool on);
+bool dcm_mp_cpusys_top_cpubiu_dbg_cg_is_on(void);
+void dcm_mp_cpusys_top_cpubiu_dbg_cg(bool on);
+bool dcm_mp_cpusys_top_cpubiu_dcm_is_on(void);
+void dcm_mp_cpusys_top_cpubiu_dcm(bool on);
+bool dcm_mp_cpusys_top_cpu_pll_div_0_dcm_is_on(void);
+void dcm_mp_cpusys_top_cpu_pll_div_0_dcm(bool on);
+bool dcm_mp_cpusys_top_cpu_pll_div_1_dcm_is_on(void);
+void dcm_mp_cpusys_top_cpu_pll_div_1_dcm(bool on);
+bool dcm_mp_cpusys_top_fcm_stall_dcm_is_on(void);
+void dcm_mp_cpusys_top_fcm_stall_dcm(bool on);
+bool dcm_mp_cpusys_top_last_cor_idle_dcm_is_on(void);
+void dcm_mp_cpusys_top_last_cor_idle_dcm(bool on);
+bool dcm_mp_cpusys_top_misc_dcm_is_on(void);
+void dcm_mp_cpusys_top_misc_dcm(bool on);
+bool dcm_mp_cpusys_top_mp0_qdcm_is_on(void);
+void dcm_mp_cpusys_top_mp0_qdcm(bool on);
+bool dcm_cpccfg_reg_emi_wfifo_is_on(void);
+void dcm_cpccfg_reg_emi_wfifo(bool on);
+#endif
diff --git a/plat/mediatek/mt8186/drivers/emi_mpu/emi_mpu.c b/plat/mediatek/mt8186/drivers/emi_mpu/emi_mpu.c
new file mode 100644
index 0000000..989ecf1
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/emi_mpu/emi_mpu.c
@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <emi_mpu.h>
+
+#if ENABLE_EMI_MPU_SW_LOCK
+static unsigned char region_lock_state[EMI_MPU_REGION_NUM];
+#endif
+
+#define EMI_MPU_START_MASK		(0x00FFFFFF)
+#define EMI_MPU_END_MASK		(0x00FFFFFF)
+#define EMI_MPU_APC_SW_LOCK_MASK	(0x00FFFFFF)
+#define EMI_MPU_APC_HW_LOCK_MASK	(0x80FFFFFF)
+
+static int _emi_mpu_set_protection(unsigned int start, unsigned int end,
+				   unsigned int apc)
+{
+	unsigned int dgroup;
+	unsigned int region;
+
+	region = (start >> 24) & 0xFF;
+	start &= EMI_MPU_START_MASK;
+	dgroup = (end >> 24) & 0xFF;
+	end &= EMI_MPU_END_MASK;
+
+	if  ((region >= EMI_MPU_REGION_NUM) || (dgroup > EMI_MPU_DGROUP_NUM)) {
+		WARN("invalid region, domain\n");
+		return -1;
+	}
+
+#if ENABLE_EMI_MPU_SW_LOCK
+	if (region_lock_state[region] == 1) {
+		WARN("invalid region\n");
+		return -1;
+	}
+
+	if ((dgroup == 0U) && ((apc >> 31) & 0x1)) {
+		region_lock_state[region] = 1;
+	}
+
+	apc &= EMI_MPU_APC_SW_LOCK_MASK;
+#else
+	apc &= EMI_MPU_APC_HW_LOCK_MASK;
+#endif
+
+	if ((start >= DRAM_OFFSET) && (end >= start)) {
+		start -= DRAM_OFFSET;
+		end -= DRAM_OFFSET;
+	} else {
+		WARN("invalid range\n");
+		return -1;
+	}
+
+	mmio_write_32(EMI_MPU_SA(region), start);
+	mmio_write_32(EMI_MPU_EA(region), end);
+	mmio_write_32(EMI_MPU_APC(region, dgroup), apc);
+
+#if defined(SUB_EMI_MPU_BASE)
+	mmio_write_32(SUB_EMI_MPU_SA(region), start);
+	mmio_write_32(SUB_EMI_MPU_EA(region), end);
+	mmio_write_32(SUB_EMI_MPU_APC(region, dgroup), apc);
+#endif
+	return 0;
+}
+
+int emi_mpu_set_protection(struct emi_region_info_t *region_info)
+{
+	unsigned int start, end;
+	int i;
+
+	if (region_info->region >= EMI_MPU_REGION_NUM) {
+		WARN("invalid region\n");
+		return -1;
+	}
+
+	start = (unsigned int)(region_info->start >> EMI_MPU_ALIGN_BITS) |
+		(region_info->region << 24);
+
+	for (i = EMI_MPU_DGROUP_NUM - 1; i >= 0; i--) {
+		end = (unsigned int)(region_info->end >> EMI_MPU_ALIGN_BITS) | (i << 24);
+
+		if (_emi_mpu_set_protection(start, end, region_info->apc[i]) < 0) {
+			WARN("failed to set emi mpu protection(%d, %d, %d)\n",
+			     start, end, region_info->apc[i]);
+		}
+	}
+
+	return 0;
+}
+
+void emi_mpu_init(void)
+{
+	/* TODO: more setting for EMI MPU. */
+}
diff --git a/plat/mediatek/mt8186/drivers/emi_mpu/emi_mpu.h b/plat/mediatek/mt8186/drivers/emi_mpu/emi_mpu.h
new file mode 100644
index 0000000..415146e
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/emi_mpu/emi_mpu.h
@@ -0,0 +1,98 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef EMI_MPU_H
+#define EMI_MPU_H
+
+#include <platform_def.h>
+
+#define ENABLE_EMI_MPU_SW_LOCK		1
+
+#define EMI_MPU_CTRL			(EMI_MPU_BASE + 0x000)
+#define EMI_MPU_DBG			(EMI_MPU_BASE + 0x004)
+#define EMI_MPU_SA0			(EMI_MPU_BASE + 0x100)
+#define EMI_MPU_EA0			(EMI_MPU_BASE + 0x200)
+#define EMI_MPU_SA(region)		(EMI_MPU_SA0 + (region * 4))
+#define EMI_MPU_EA(region)		(EMI_MPU_EA0 + (region * 4))
+#define EMI_MPU_APC0			(EMI_MPU_BASE + 0x300)
+#define EMI_MPU_APC(region, dgroup)	(EMI_MPU_APC0 + (region * 4) + (dgroup * 0x100))
+#define EMI_MPU_CTRL_D0			(EMI_MPU_BASE + 0x800)
+#define EMI_MPU_CTRL_D(domain)		(EMI_MPU_CTRL_D0 + (domain * 4))
+#define EMI_RG_MASK_D0			(EMI_MPU_BASE + 0x900)
+#define EMI_RG_MASK_D(domain)		(EMI_RG_MASK_D0 + (domain * 4))
+#define EMI_MPU_START			(0x000)
+#define EMI_MPU_END			(0x93C)
+
+#define SUB_EMI_MPU_CTRL		(SUB_EMI_MPU_BASE + 0x000)
+#define SUB_EMI_MPU_DBG			(SUB_EMI_MPU_BASE + 0x004)
+#define SUB_EMI_MPU_SA0			(SUB_EMI_MPU_BASE + 0x100)
+#define SUB_EMI_MPU_EA0			(SUB_EMI_MPU_BASE + 0x200)
+#define SUB_EMI_MPU_SA(region)		(SUB_EMI_MPU_SA0 + (region * 4))
+#define SUB_EMI_MPU_EA(region)		(SUB_EMI_MPU_EA0 + (region * 4))
+#define SUB_EMI_MPU_APC0		(SUB_EMI_MPU_BASE + 0x300)
+#define SUB_EMI_MPU_APC(region, dgroup)	(SUB_EMI_MPU_APC0 + (region * 4) + (dgroup * 0x100))
+#define SUB_EMI_MPU_CTRL_D0		(SUB_EMI_MPU_BASE + 0x800)
+#define SUB_EMI_MPU_CTRL_D(domain)	(SUB_EMI_MPU_CTRL_D0 + (domain * 4))
+#define SUB_EMI_RG_MASK_D0		(SUB_EMI_MPU_BASE + 0x900)
+#define SUB_EMI_RG_MASK_D(domain)	(SUB_EMI_RG_MASK_D0 + (domain * 4))
+
+#define EMI_MPU_DOMAIN_NUM		(16)
+#define EMI_MPU_REGION_NUM		(32)
+#define EMI_MPU_ALIGN_BITS		(16)
+#define DRAM_OFFSET			(0x40000000 >> EMI_MPU_ALIGN_BITS)
+
+#define NO_PROTECTION			0
+#define SEC_RW				1
+#define SEC_RW_NSEC_R			2
+#define SEC_RW_NSEC_W			3
+#define SEC_R_NSEC_R			4
+#define FORBIDDEN			5
+#define SEC_R_NSEC_RW			6
+
+#define LOCK				1
+#define UNLOCK				0
+
+#define EMI_MPU_DGROUP_NUM		(EMI_MPU_DOMAIN_NUM / 8)
+
+#if (EMI_MPU_DGROUP_NUM == 1)
+#define SET_ACCESS_PERMISSION(apc_ary, lock, d7, d6, d5, d4, d3, d2, d1, d0) \
+do { \
+	apc_ary[1] = 0; \
+	apc_ary[0] = \
+		(((unsigned int)  d7) << 21) | (((unsigned int)  d6) << 18) | \
+		(((unsigned int)  d5) << 15) | (((unsigned int)  d4) << 12) | \
+		(((unsigned int)  d3) <<  9) | (((unsigned int)  d2) <<  6) | \
+		(((unsigned int)  d1) <<  3) |  ((unsigned int)  d0) | \
+		((unsigned int) lock << 31); \
+} while (0)
+#elif (EMI_MPU_DGROUP_NUM == 2)
+#define SET_ACCESS_PERMISSION(apc_ary, lock, d15, d14, d13, d12, d11, d10, \
+				d9, d8, d7, d6, d5, d4, d3, d2, d1, d0) \
+do { \
+	apc_ary[1] = \
+		(((unsigned int) d15) << 21) | (((unsigned int) d14) << 18) | \
+		(((unsigned int) d13) << 15) | (((unsigned int) d12) << 12) | \
+		(((unsigned int) d11) <<  9) | (((unsigned int) d10) <<  6) | \
+		(((unsigned int)  d9) <<  3) |  ((unsigned int)  d8); \
+	apc_ary[0] = \
+		(((unsigned int)  d7) << 21) | (((unsigned int)  d6) << 18) | \
+		(((unsigned int)  d5) << 15) | (((unsigned int)  d4) << 12) | \
+		(((unsigned int)  d3) <<  9) | (((unsigned int)  d2) <<  6) | \
+		(((unsigned int)  d1) <<  3) |  ((unsigned int)  d0) | \
+		((unsigned int) lock << 31); \
+} while (0)
+#endif
+
+struct emi_region_info_t {
+	unsigned long long start;
+	unsigned long long end;
+	unsigned int region;
+	unsigned int apc[EMI_MPU_DGROUP_NUM];
+};
+
+void emi_mpu_init(void);
+
+#endif
diff --git a/plat/mediatek/mt8186/drivers/gpio/mtgpio.c b/plat/mediatek/mt8186/drivers/gpio/mtgpio.c
new file mode 100644
index 0000000..134476a
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/gpio/mtgpio.c
@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <mtgpio.h>
+#include <platform_def.h>
+
+uintptr_t mt_gpio_find_reg_addr(uint32_t pin)
+{
+	uintptr_t reg_addr = 0U;
+	struct mt_pin_info gpio_info;
+
+	assert(pin < MAX_GPIO_PIN);
+
+	gpio_info = mt_pin_infos[pin];
+
+	switch (gpio_info.base & 0x0f) {
+	case 0:
+		reg_addr = IOCFG_LT_BASE;
+		break;
+	case 1:
+		reg_addr = IOCFG_LM_BASE;
+		break;
+	case 2:
+		reg_addr = IOCFG_LB_BASE;
+		break;
+	case 3:
+		reg_addr = IOCFG_BL_BASE;
+		break;
+	case 4:
+		reg_addr = IOCFG_RB_BASE;
+		break;
+	case 5:
+		reg_addr = IOCFG_RT_BASE;
+		break;
+	default:
+		break;
+	}
+
+	return reg_addr;
+}
diff --git a/plat/mediatek/mt8186/drivers/gpio/mtgpio.h b/plat/mediatek/mt8186/drivers/gpio/mtgpio.h
new file mode 100644
index 0000000..4430a58
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/gpio/mtgpio.h
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MT_GPIO_H
+#define MT_GPIO_H
+
+#include <mtgpio_common.h>
+
+/* Enumeration for GPIO pin */
+typedef enum GPIO_PIN {
+	GPIO_UNSUPPORTED = -1,
+	GPIO0, GPIO1, GPIO2, GPIO3, GPIO4, GPIO5, GPIO6,
+	GPIO7, GPIO8, GPIO9, GPIO10, GPIO11, GPIO12, GPIO13, GPIO14,
+	GPIO15, GPIO16, GPIO17, GPIO18, GPIO19, GPIO20, GPIO21, GPIO22,
+	GPIO23, GPIO24, GPIO25, GPIO26, GPIO27, GPIO28, GPIO29, GPIO30,
+	GPIO31, GPIO32, GPIO33, GPIO34, GPIO35, GPIO36, GPIO37, GPIO38,
+	GPIO39, GPIO40, GPIO41, GPIO42, GPIO43, GPIO44, GPIO45, GPIO46,
+	GPIO47, GPIO48, GPIO49, GPIO50, GPIO51, GPIO52, GPIO53, GPIO54,
+	GPIO55, GPIO56, GPIO57, GPIO58, GPIO59, GPIO60, GPIO61, GPIO62,
+	GPIO63, GPIO64, GPIO65, GPIO66, GPIO67, GPIO68, GPIO69, GPIO70,
+	GPIO71, GPIO72, GPIO73, GPIO74, GPIO75, GPIO76, GPIO77, GPIO78,
+	GPIO79, GPIO80, GPIO81, GPIO82, GPIO83, GPIO84, GPIO85, GPIO86,
+	GPIO87, GPIO88, GPIO89, GPIO90, GPIO91, GPIO92, GPIO93, GPIO94,
+	GPIO95, GPIO96, GPIO97, GPIO98, GPIO99, GPIO100, GPIO101, GPIO102,
+	GPIO103, GPIO104, GPIO105, GPIO106, GPIO107, GPIO108, GPIO109, GPIO110,
+	GPIO111, GPIO112, GPIO113, GPIO114, GPIO115, GPIO116, GPIO117, GPIO118,
+	GPIO119, GPIO120, GPIO121, GPIO122, GPIO123, GPIO124, GPIO125, GPIO126,
+	GPIO127, GPIO128, GPIO129, GPIO130, GPIO131, GPIO132, GPIO133, GPIO134,
+	GPIO135, GPIO136, GPIO137, GPIO138, GPIO139, GPIO140, GPIO141, GPIO142,
+	GPIO143, GPIO144, GPIO145, GPIO146, GPIO147, GPIO148, GPIO149, GPIO150,
+	GPIO151, GPIO152, GPIO153, GPIO154, GPIO155, GPIO156, GPIO157, GPIO158,
+	GPIO159, GPIO160, GPIO161, GPIO162, GPIO163, GPIO164, GPIO165, GPIO166,
+	GPIO167, GPIO168, GPIO169, GPIO170, GPIO171, GPIO172, GPIO173, GPIO174,
+	GPIO175, GPIO176, GPIO177, GPIO178, GPIO179, GPIO180, GPIO181, GPIO182,
+	GPIO183, GPIO184,
+	MT_GPIO_BASE_MAX
+} GPIO_PIN;
+
+static const struct mt_pin_info mt_pin_infos[] = {
+	PIN(0, 0, 13, 0x16, 0x40),
+	PIN(1, 0, 14, 0x16, 0x40),
+	PIN(2, 0, 17, 0x16, 0x40),
+	PIN(3, 0, 18, 0x16, 0x40),
+	PIN(4, 0, 19, 0x16, 0x40),
+	PIN(5, 0, 20, 0x16, 0x40),
+	PIN(6, 0, 19, 0x24, 0x40),
+	PIN(7, 0, 20, 0x24, 0x40),
+	PIN(8, 0, 21, 0x24, 0x40),
+	PIN(9, 0, 22, 0x24, 0x40),
+	PIN(10, 0, 16, 0x24, 0x40),
+	PIN(11, 0, 17, 0x24, 0x40),
+	PIN(12, 0, 18, 0x24, 0x40),
+	PIN(13, 0, 0, 0x23, 0x60),
+	PIN(14, 0, 1, 0x23, 0x60),
+	PIN(15, 0, 15, 0x16, 0x40),
+	PIN(16, 0, 16, 0x16, 0x40),
+	PIN(17, 0, 9, 0x25, 0x70),
+	PIN(18, 0, 10, 0x25, 0x70),
+	PIN(19, 0, 3, 0x25, 0x70),
+	PIN(20, 0, 6, 0x25, 0x70),
+	PIN(21, 0, 4, 0x25, 0x70),
+	PIN(22, 0, 7, 0x25, 0x70),
+	PIN(23, 0, 5, 0x25, 0x70),
+	PIN(24, 0, 8, 0x25, 0x70),
+	PIN(25, 0, 18, 0x25, 0x70),
+	PIN(26, 0, 15, 0x25, 0x70),
+	PIN(27, 0, 17, 0x25, 0x70),
+	PIN(28, 0, 16, 0x25, 0x70),
+	PIN(29, 0, 0, 0x16, 0x40),
+	PIN(30, 0, 1, 0x16, 0x40),
+	PIN(31, 0, 2, 0x16, 0x40),
+	PIN(32, 0, 25, 0x12, 0x50),
+	PIN(33, 0, 27, 0x12, 0x50),
+	PIN(34, 0, 26, 0x12, 0x50),
+	PIN(35, 0, 28, 0x12, 0x50),
+	PIN(36, 0, 9, 0x12, 0x50),
+	PIN(37, 0, 10, 0x12, 0x50),
+	PIN(38, 0, 12, 0x12, 0x50),
+	PIN(39, 0, 11, 0x12, 0x50),
+	PIN(40, 0, 13, 0x12, 0x50),
+	PIN(41, 0, 14, 0x12, 0x50),
+	PIN(42, 0, 16, 0x12, 0x50),
+	PIN(43, 0, 15, 0x12, 0x50),
+	PIN(44, 0, 28, 0x25, 0x70),
+	PIN(45, 0, 29, 0x25, 0x70),
+	PIN(46, 0, 31, 0x25, 0x70),
+	PIN(47, 0, 30, 0x25, 0x70),
+	PIN(48, 0, 17, 0x12, 0x50),
+	PIN(49, 0, 18, 0x12, 0x50),
+	PIN(50, 0, 20, 0x12, 0x50),
+	PIN(51, 0, 19, 0x12, 0x50),
+	PIN(52, 0, 12, 0x23, 0x60),
+	PIN(53, 0, 13, 0x23, 0x60),
+	PIN(54, 0, 15, 0x23, 0x60),
+	PIN(55, 0, 14, 0x23, 0x60),
+	PIN(56, 0, 12, 0x25, 0x70),
+	PIN(57, 0, 11, 0x25, 0x70),
+	PIN(58, 0, 13, 0x25, 0x70),
+	PIN(59, 0, 14, 0x25, 0x70),
+	PIN(60, 0, 21, 0x23, 0x60),
+	PIN(61, 0, 16, 0x23, 0x60),
+	PIN(62, 0, 22, 0x23, 0x60),
+	PIN(63, 0, 17, 0x23, 0x60),
+	PIN(64, 0, 18, 0x23, 0x60),
+	PIN(65, 0, 19, 0x23, 0x60),
+	PIN(66, 0, 20, 0x23, 0x60),
+	PIN(67, 1, 10, 0x21, 0x70),
+	PIN(68, 1, 0, 0x21, 0x70),
+	PIN(69, 1, 1, 0x21, 0x70),
+	PIN(70, 1, 11, 0x21, 0x70),
+	PIN(71, 1, 2, 0x21, 0x70),
+	PIN(72, 1, 3, 0x21, 0x70),
+	PIN(73, 1, 4, 0x21, 0x70),
+	PIN(74, 1, 5, 0x21, 0x70),
+	PIN(75, 1, 6, 0x21, 0x70),
+	PIN(76, 1, 7, 0x21, 0x70),
+	PIN(77, 1, 8, 0x21, 0x70),
+	PIN(78, 1, 9, 0x21, 0x70),
+	PIN(79, 1, 0, 0x25, 0x80),
+	PIN(80, 1, 1, 0x25, 0x80),
+	PIN(81, 1, 2, 0x25, 0x80),
+	PIN(82, 1, 3, 0x25, 0x80),
+	PIN(83, 0, 3, 0x16, 0x40),
+	PIN(84, 1, 0, 0x23, 0x70),
+	PIN(85, 1, 1, 0x23, 0x70),
+	PIN(86, 1, 2, 0x23, 0x70),
+	PIN(87, 1, 3, 0x23, 0x70),
+	PIN(88, 1, 4, 0x23, 0x70),
+	PIN(89, 1, 5, 0x23, 0x70),
+	PIN(90, 0, 2, 0x23, 0x60),
+	PIN(91, 0, 23, 0x23, 0x60),
+	PIN(92, 0, 25, 0x23, 0x60),
+	PIN(93, 0, 3, 0x23, 0x60),
+	PIN(94, 0, 24, 0x23, 0x60),
+	PIN(95, 0, 26, 0x23, 0x60),
+	PIN(96, 0, 1, 0x12, 0x50),
+	PIN(97, 0, 0, 0x12, 0x50),
+	PIN(98, 0, 2, 0x12, 0x50),
+	PIN(99, 0, 14, 0x24, 0x40),
+	PIN(100, 0, 15, 0x24, 0x40),
+	PIN(101, 0, 13, 0x24, 0x40),
+	PIN(102, 0, 12, 0x24, 0x40),
+	PIN(103, 0, 0, 0x24, 0x40),
+	PIN(104, 0, 1, 0x24, 0x40),
+	PIN(105, 0, 4, 0x24, 0x40),
+	PIN(106, 0, 5, 0x24, 0x40),
+	PIN(107, 0, 6, 0x24, 0x40),
+	PIN(108, 0, 7, 0x24, 0x40),
+	PIN(109, 0, 8, 0x24, 0x40),
+	PIN(110, 0, 9, 0x24, 0x40),
+	PIN(111, 0, 10, 0x24, 0x40),
+	PIN(112, 0, 11, 0x24, 0x40),
+	PIN(113, 0, 2, 0x24, 0x40),
+	PIN(114, 0, 3, 0x24, 0x40),
+	PIN(115, 0, 4, 0x23, 0x60),
+	PIN(116, 0, 7, 0x23, 0x60),
+	PIN(117, 0, 5, 0x23, 0x60),
+	PIN(118, 0, 6, 0x23, 0x60),
+	PIN(119, 0, 22, 0x25, 0x70),
+	PIN(120, 0, 19, 0x25, 0x70),
+	PIN(121, 0, 20, 0x25, 0x70),
+	PIN(122, 0, 21, 0x25, 0x70),
+	PIN(123, 0, 23, 0x25, 0x70),
+	PIN(124, 0, 0, 0x25, 0x70),
+	PIN(125, 0, 1, 0x25, 0x70),
+	PIN(126, 0, 2, 0x25, 0x70),
+	PIN(127, 0, 8, 0x23, 0x60),
+	PIN(128, 0, 10, 0x23, 0x60),
+	PIN(129, 0, 24, 0x25, 0x70),
+	PIN(130, 0, 26, 0x25, 0x70),
+	PIN(131, 0, 25, 0x25, 0x70),
+	PIN(132, 0, 27, 0x25, 0x70),
+	PIN(133, 0, 9, 0x21, 0x60),
+	PIN(134, 0, 12, 0x21, 0x60),
+	PIN(135, 0, 21, 0x16, 0x40),
+	PIN(136, 0, 24, 0x16, 0x40),
+	PIN(137, 0, 10, 0x21, 0x60),
+	PIN(138, 0, 13, 0x21, 0x60),
+	PIN(139, 0, 7, 0x12, 0x50),
+	PIN(140, 0, 8, 0x12, 0x50),
+	PIN(141, 0, 9, 0x23, 0x60),
+	PIN(142, 0, 11, 0x23, 0x60),
+	PIN(143, 0, 22, 0x16, 0x40),
+	PIN(144, 0, 25, 0x16, 0x40),
+	PIN(145, 0, 23, 0x16, 0x40),
+	PIN(146, 0, 26, 0x16, 0x40),
+	PIN(147, 0, 23, 0x24, 0x40),
+	PIN(148, 0, 24, 0x24, 0x40),
+	PIN(149, 0, 25, 0x24, 0x40),
+	PIN(150, 0, 26, 0x24, 0x40),
+	PIN(151, 0, 27, 0x24, 0x40),
+	PIN(152, 0, 28, 0x24, 0x40),
+	PIN(153, 0, 29, 0x24, 0x40),
+	PIN(154, 0, 30, 0x24, 0x40),
+	PIN(155, 0, 31, 0x24, 0x40),
+	PIN(156, 0, 0, 0x24, 0x50),
+	PIN(157, 0, 4, 0x12, 0x50),
+	PIN(158, 0, 3, 0x12, 0x50),
+	PIN(159, 0, 6, 0x12, 0x50),
+	PIN(160, 0, 5, 0x12, 0x50),
+	PIN(161, 0, 23, 0x12, 0x50),
+	PIN(162, 0, 24, 0x12, 0x50),
+	PIN(163, 0, 11, 0x21, 0x60),
+	PIN(164, 0, 8, 0x21, 0x60),
+	PIN(165, 0, 16, 0x21, 0x60),
+	PIN(166, 0, 1, 0x21, 0x60),
+	PIN(167, 0, 7, 0x21, 0x60),
+	PIN(168, 0, 4, 0x21, 0x60),
+	PIN(169, 0, 5, 0x21, 0x60),
+	PIN(170, 0, 0, 0x21, 0x60),
+	PIN(171, 0, 6, 0x21, 0x60),
+	PIN(172, 0, 2, 0x21, 0x60),
+	PIN(173, 0, 3, 0x21, 0x60),
+	PIN(174, 0, 7, 0x16, 0x40),
+	PIN(175, 0, 8, 0x16, 0x40),
+	PIN(176, 0, 4, 0x16, 0x40),
+	PIN(177, 0, 5, 0x16, 0x40),
+	PIN(178, 0, 6, 0x16, 0x40),
+	PIN(179, 0, 9, 0x16, 0x40),
+	PIN(180, 0, 10, 0x16, 0x40),
+	PIN(181, 0, 11, 0x16, 0x40),
+	PIN(182, 0, 12, 0x16, 0x40),
+	PIN(183, 0, 21, 0x12, 0x50),
+	PIN(184, 0, 22, 0x12, 0x50),
+};
+
+#endif /* MT_GPIO_H */
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm.c b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm.c
new file mode 100644
index 0000000..c6c2e38
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm.c
@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdint.h>
+
+#include <arch_helpers.h>
+#include <lib/psci/psci.h>
+#include <lib/spinlock.h>
+
+#include <mt_cpu_pm_cpc.h>
+#include <mt_mcdi.h>
+#include <plat_mtk_lpm.h>
+#include <plat_pm.h>
+
+DEFINE_SYSREG_RW_FUNCS(dbgprcr_el1);
+
+static int plat_mt_lp_cpu_rc;
+
+static int pwr_state_prompt(unsigned int cpu, const psci_power_state_t *state)
+{
+	return 0;
+}
+
+static int pwr_state_reflect(unsigned int cpu, const psci_power_state_t *state)
+{
+	mtk_cpc_core_on_hint_clr(cpu);
+
+	if (IS_SYSTEM_SUSPEND_STATE(state)) {
+		mtk_cpc_time_sync();
+	}
+
+	return 0;
+}
+
+static int pwr_cpu_pwron(unsigned int cpu, const psci_power_state_t *state)
+{
+	return 0;
+}
+
+static int pwr_cpu_pwrdwn(unsigned int cpu, const psci_power_state_t *state)
+{
+	/* clear DBGPRCR.CORENPDRQ to allow CPU power down  */
+	write_dbgprcr_el1(0ULL);
+
+	return 0;
+}
+
+static int pwr_cluster_pwron(unsigned int cpu, const psci_power_state_t *state)
+{
+	return 0;
+}
+
+static int pwr_cluster_pwrdwn(unsigned int cpu, const psci_power_state_t *state)
+{
+	return 0;
+}
+
+static int pwr_mcusys_pwron(unsigned int cpu, const psci_power_state_t *state)
+{
+	if (!IS_MCUSYS_OFF_STATE(state) || (plat_mt_lp_cpu_rc < 0)) {
+		return -1;
+	}
+
+	mtk_cpc_mcusys_off_reflect();
+
+	return 0;
+}
+
+static int pwr_mcusys_pwron_finished(unsigned int cpu,
+					const psci_power_state_t *state)
+{
+	if (!IS_MCUSYS_OFF_STATE(state) || (plat_mt_lp_cpu_rc < 0)) {
+		return -1;
+	}
+
+	return 0;
+}
+
+static int pwr_mcusys_pwrdwn(unsigned int cpu, const psci_power_state_t *state)
+{
+	if (!IS_MCUSYS_OFF_STATE(state)) {
+		goto mt_pwr_mcusysoff_break;
+	}
+
+	if (mcdi_try_init() != 0) { /* not ready to process mcusys-off */
+		goto mt_pwr_mcusysoff_break;
+	}
+
+	return 0;
+
+mt_pwr_mcusysoff_break:
+
+	plat_mt_lp_cpu_rc = -1;
+
+	return -1;
+}
+
+static const struct mt_lpm_tz plat_pm = {
+	.pwr_prompt			= pwr_state_prompt,
+	.pwr_reflect			= pwr_state_reflect,
+	.pwr_cpu_on			= pwr_cpu_pwron,
+	.pwr_cpu_dwn			= pwr_cpu_pwrdwn,
+	.pwr_cluster_on			= pwr_cluster_pwron,
+	.pwr_cluster_dwn		= pwr_cluster_pwrdwn,
+	.pwr_mcusys_dwn			= pwr_mcusys_pwrdwn,
+	.pwr_mcusys_on			= pwr_mcusys_pwron,
+	.pwr_mcusys_on_finished		= pwr_mcusys_pwron_finished
+};
+
+const struct mt_lpm_tz *mt_plat_cpu_pm_init(void)
+{
+	mtk_cpc_init();
+
+	if (mcdi_try_init() == 0) {
+		INFO("MCDI init done.\n");
+	}
+
+	return &plat_pm;
+}
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm.h b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm.h
new file mode 100644
index 0000000..83a7a53
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef __MT_CPU_PM_H__
+#define __MT_CPU_PM_H__
+
+#define MCUSYS_STATUS_PDN		(1 << 0UL)
+#define MCUSYS_STATUS_CPUSYS_PROTECT	(1 << 8UL)
+#define MCUSYS_STATUS_MCUSYS_PROTECT	(1 << 9UL)
+
+/* cpu_pm function ID*/
+enum mt_cpu_pm_user_id {
+	MCUSYS_STATUS,
+	CPC_COMMAND,
+	IRQ_REMAIN_LIST_ALLOC,
+	IRQ_REMAIN_IRQ_ADD,
+	IRQ_REMAIN_IRQ_SUBMIT,
+	MBOX_INFO,
+};
+
+/* cpu_pm lp function ID */
+enum mt_cpu_pm_lp_smc_id {
+	LP_CPC_COMMAND,
+	IRQS_REMAIN_ALLOC,
+	IRQS_REMAIN_CTRL,
+	IRQS_REMAIN_IRQ,
+	IRQS_REMAIN_WAKEUP_CAT,
+	IRQS_REMAIN_WAKEUP_SRC,
+};
+
+#endif
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_cpc.c b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_cpc.c
new file mode 100644
index 0000000..2b0f071
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_cpc.c
@@ -0,0 +1,269 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+
+#include <drivers/delay_timer.h>
+
+#include <mt_cpu_pm_cpc.h>
+#include <mt_timer.h>
+
+struct mtk_cpc_dev {
+	int auto_off;
+	unsigned int auto_thres_tick;
+};
+
+static struct mtk_cpc_dev cpc;
+
+static int mtk_cpc_last_core_prot(uint32_t prot_req,
+				uint32_t resp_reg, uint32_t resp_ofs)
+{
+	uint32_t sta, retry;
+
+	retry = 0U;
+
+	while (retry++ < RETRY_CNT_MAX) {
+
+		mmio_write_32(CPC_MCUSYS_LAST_CORE_REQ, prot_req);
+
+		udelay(1U);
+
+		sta = (mmio_read_32(resp_reg) >> resp_ofs) & CPC_PROT_RESP_MASK;
+
+		if (sta == PROT_SUCCESS) {
+			return CPC_SUCCESS;
+		} else if (sta == PROT_GIVEUP) {
+			return CPC_ERR_FAIL;
+		}
+	}
+
+	return CPC_ERR_TIMEOUT;
+}
+
+int mtk_cpu_pm_mcusys_prot_aquire(void)
+{
+	return mtk_cpc_last_core_prot(
+			MCUSYS_PROT_SET,
+			CPC_MCUSYS_LAST_CORE_RESP,
+			MCUSYS_RESP_OFS);
+}
+
+void mtk_cpu_pm_mcusys_prot_release(void)
+{
+	mmio_write_32(CPC_MCUSYS_PWR_ON_MASK, MCUSYS_PROT_CLR);
+}
+
+int mtk_cpu_pm_cluster_prot_aquire(unsigned int cluster)
+{
+	return mtk_cpc_last_core_prot(
+			CPUSYS_PROT_SET,
+			CPC_MCUSYS_MP_LAST_CORE_RESP,
+			CPUSYS_RESP_OFS);
+}
+
+void mtk_cpu_pm_cluster_prot_release(unsigned int cluster)
+{
+	mmio_write_32(CPC_MCUSYS_PWR_ON_MASK, CPUSYS_PROT_CLR);
+}
+
+static void mtk_cpc_cluster_cnt_backup(void)
+{
+	uint32_t backup_cnt;
+	uint32_t curr_cnt;
+	uint32_t cnt_mask = GENMASK(14, 0);
+	uint32_t clr_mask = GENMASK(1, 0);
+
+	/* Single Cluster */
+	backup_cnt = mmio_read_32(CPC_CLUSTER_CNT_BACKUP);
+	curr_cnt = mmio_read_32(CPC_MCUSYS_CLUSTER_COUNTER);
+
+	/* Get off count if dormant count is 0 */
+	if ((curr_cnt & cnt_mask) == 0U) {
+		curr_cnt = (curr_cnt >> 16) & cnt_mask;
+	} else {
+		curr_cnt = curr_cnt & cnt_mask;
+	}
+
+	mmio_write_32(CPC_CLUSTER_CNT_BACKUP, backup_cnt + curr_cnt);
+	mmio_write_32(CPC_MCUSYS_CLUSTER_COUNTER_CLR, clr_mask);
+}
+
+static inline void mtk_cpc_mcusys_off_en(void)
+{
+	mmio_write_32(CPC_MCUSYS_PWR_CTRL, 1U);
+}
+
+static inline void mtk_cpc_mcusys_off_dis(void)
+{
+	mmio_write_32(CPC_MCUSYS_PWR_CTRL, 0U);
+}
+
+void mtk_cpc_mcusys_off_reflect(void)
+{
+	mtk_cpc_mcusys_off_dis();
+	mtk_cpu_pm_mcusys_prot_release();
+}
+
+int mtk_cpc_mcusys_off_prepare(void)
+{
+	if (mtk_cpu_pm_mcusys_prot_aquire() != CPC_SUCCESS) {
+		return CPC_ERR_FAIL;
+	}
+
+	mtk_cpc_cluster_cnt_backup();
+	mtk_cpc_mcusys_off_en();
+
+	return CPC_SUCCESS;
+}
+
+void mtk_cpc_core_on_hint_set(unsigned int cpu)
+{
+	mmio_write_32(CPC_MCUSYS_CPU_ON_SW_HINT_SET, BIT(cpu));
+}
+
+void mtk_cpc_core_on_hint_clr(unsigned int cpu)
+{
+	mmio_write_32(CPC_MCUSYS_CPU_ON_SW_HINT_CLR, BIT(cpu));
+}
+
+static void mtk_cpc_dump_timestamp(void)
+{
+	uint32_t id;
+
+	for (id = 0U; id < CPC_TRACE_ID_NUM; id++) {
+		mmio_write_32(CPC_MCUSYS_TRACE_SEL, id);
+
+		memcpy((void *)(uintptr_t)CPC_TRACE_SRAM(id),
+				(const void *)(uintptr_t)CPC_MCUSYS_TRACE_DATA,
+				CPC_TRACE_SIZE);
+	}
+}
+
+void mtk_cpc_time_sync(void)
+{
+	uint64_t kt;
+	uint32_t systime_l, systime_h;
+
+	kt = sched_clock();
+	systime_l = mmio_read_32(CNTSYS_L_REG);
+	systime_h = mmio_read_32(CNTSYS_H_REG);
+
+	/* sync kernel timer to cpc */
+	mmio_write_32(CPC_MCUSYS_CPC_KERNEL_TIME_L_BASE, (uint32_t)kt);
+	mmio_write_32(CPC_MCUSYS_CPC_KERNEL_TIME_H_BASE, (uint32_t)(kt >> 32));
+	/* sync system timer to cpc */
+	mmio_write_32(CPC_MCUSYS_CPC_SYSTEM_TIME_L_BASE, systime_l);
+	mmio_write_32(CPC_MCUSYS_CPC_SYSTEM_TIME_H_BASE, systime_h);
+}
+
+static void mtk_cpc_config(uint32_t cfg, uint32_t data)
+{
+	uint32_t val;
+	uint32_t reg = 0U;
+
+	switch (cfg) {
+	case CPC_SMC_CONFIG_PROF:
+		reg = CPC_MCUSYS_CPC_DBG_SETTING;
+		val = mmio_read_32(reg);
+		val = (data != 0U) ? (val | CPC_PROF_EN) : (val & ~CPC_PROF_EN);
+		break;
+	case CPC_SMC_CONFIG_AUTO_OFF:
+		reg = CPC_MCUSYS_CPC_FLOW_CTRL_CFG;
+		val = mmio_read_32(reg);
+		if (data != 0U) {
+			val |= CPC_AUTO_OFF_EN;
+			cpc.auto_off = 1;
+		} else {
+			val &= ~CPC_AUTO_OFF_EN;
+			cpc.auto_off = 0;
+		}
+		break;
+	case CPC_SMC_CONFIG_AUTO_OFF_THRES:
+		reg = CPC_MCUSYS_CPC_OFF_THRES;
+		cpc.auto_thres_tick = us_to_ticks(data);
+		val = cpc.auto_thres_tick;
+		break;
+	case CPC_SMC_CONFIG_CNT_CLR:
+		reg = CPC_MCUSYS_CLUSTER_COUNTER_CLR;
+		val = GENMASK(1, 0);	/* clr_mask */
+		break;
+	case CPC_SMC_CONFIG_TIME_SYNC:
+		mtk_cpc_time_sync();
+		break;
+	default:
+		break;
+	}
+
+	if (reg != 0U) {
+		mmio_write_32(reg, val);
+	}
+}
+
+static uint32_t mtk_cpc_read_config(uint32_t cfg)
+{
+	uint32_t res = 0U;
+
+	switch (cfg) {
+	case CPC_SMC_CONFIG_PROF:
+		res = (mmio_read_32(CPC_MCUSYS_CPC_DBG_SETTING) & CPC_PROF_EN) ?
+			1U : 0U;
+		break;
+	case CPC_SMC_CONFIG_AUTO_OFF:
+		res = cpc.auto_off;
+		break;
+	case CPC_SMC_CONFIG_AUTO_OFF_THRES:
+		res = ticks_to_us(cpc.auto_thres_tick);
+		break;
+	case CPC_SMC_CONFIG_CNT_CLR:
+		break;
+	default:
+		break;
+	}
+
+	return res;
+}
+
+uint64_t mtk_cpc_handler(uint64_t act, uint64_t arg1, uint64_t arg2)
+{
+	uint64_t res = 0ULL;
+
+	switch (act) {
+	case CPC_SMC_EVENT_DUMP_TRACE_DATA:
+		mtk_cpc_dump_timestamp();
+		break;
+	case CPC_SMC_EVENT_GIC_DPG_SET:
+		/* isolated_status = x2; */
+		break;
+	case CPC_SMC_EVENT_CPC_CONFIG:
+		mtk_cpc_config((uint32_t)arg1, (uint32_t)arg2);
+		break;
+	case CPC_SMC_EVENT_READ_CONFIG:
+		res = mtk_cpc_read_config((uint32_t)arg1);
+		break;
+	default:
+		break;
+	}
+
+	return res;
+}
+
+void mtk_cpc_init(void)
+{
+	mmio_write_32(CPC_MCUSYS_CPC_DBG_SETTING,
+			mmio_read_32(CPC_MCUSYS_CPC_DBG_SETTING)
+			| CPC_DBG_EN
+			| CPC_CALC_EN);
+
+	cpc.auto_off = 1;
+	cpc.auto_thres_tick = us_to_ticks(8000);
+
+	mmio_write_32(CPC_MCUSYS_CPC_FLOW_CTRL_CFG,
+			mmio_read_32(CPC_MCUSYS_CPC_FLOW_CTRL_CFG)
+			| CPC_OFF_PRE_EN
+			| (cpc.auto_off ? CPC_AUTO_OFF_EN : 0U));
+
+	mmio_write_32(CPC_MCUSYS_CPC_OFF_THRES, cpc.auto_thres_tick);
+}
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_cpc.h b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_cpc.h
new file mode 100644
index 0000000..488b1d1
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_cpc.h
@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MT_CPU_PM_CPC_H
+#define MT_CPU_PM_CPC_H
+
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <mcucfg.h>
+#include <platform_def.h>
+
+#define NEED_CPUSYS_PROT_WORKAROUND	1
+
+/* system sram registers */
+#define CPUIDLE_SRAM_REG(r)	(0x11B000 + (r))
+
+/* db dump */
+#define CPC_TRACE_SIZE		U(0x20)
+#define CPC_TRACE_ID_NUM	U(10)
+#define CPC_TRACE_SRAM(id)	(CPUIDLE_SRAM_REG(0x10) + (id) * CPC_TRACE_SIZE)
+
+/* buckup off count */
+#define CPC_CLUSTER_CNT_BACKUP	CPUIDLE_SRAM_REG(0x1F0)
+#define CPC_MCUSYS_CNT		CPUIDLE_SRAM_REG(0x1F4)
+
+/* CPC_MCUSYS_CPC_FLOW_CTRL_CFG(0xA814): debug setting */
+#define CPC_PWR_ON_SEQ_DIS	BIT(1)
+#define CPC_PWR_ON_PRIORITY	BIT(2)
+#define CPC_AUTO_OFF_EN		BIT(5)
+#define CPC_DORMANT_WAIT_EN	BIT(14)
+#define CPC_CTRL_EN		BIT(16)
+#define CPC_OFF_PRE_EN		BIT(29)
+
+/* CPC_MCUSYS_LAST_CORE_REQ(0xA818) : last core protection */
+#define CPUSYS_PROT_SET		BIT(0)
+#define MCUSYS_PROT_SET		BIT(8)
+#define CPUSYS_PROT_CLR		BIT(8)
+#define MCUSYS_PROT_CLR		BIT(9)
+
+#define CPC_PROT_RESP_MASK	U(0x3)
+#define CPUSYS_RESP_OFS		U(16)
+#define MCUSYS_RESP_OFS		U(30)
+
+#define cpusys_resp(r)		(((r) >> CPUSYS_RESP_OFS) & CPC_PROT_RESP_MASK)
+#define mcusys_resp(r)		(((r) >> MCUSYS_RESP_OFS) & CPC_PROT_RESP_MASK)
+
+#define RETRY_CNT_MAX		U(1000)
+
+#define PROT_RETRY		U(0)
+#define PROT_SUCCESS		U(1)
+#define PROT_GIVEUP		U(2)
+
+/* CPC_MCUSYS_CPC_DBG_SETTING(0xAB00): debug setting */
+#define CPC_PROF_EN		BIT(0)
+#define CPC_DBG_EN		BIT(1)
+#define CPC_FREEZE		BIT(2)
+#define CPC_CALC_EN		BIT(3)
+
+enum {
+	CPC_SUCCESS = 0U,
+	CPC_ERR_FAIL = 1U,
+	CPC_ERR_TIMEOUT = 2U,
+	NF_CPC_ERR = 3U,
+};
+
+enum {
+	CPC_SMC_EVENT_DUMP_TRACE_DATA = 0U,
+	CPC_SMC_EVENT_GIC_DPG_SET = 1U,
+	CPC_SMC_EVENT_CPC_CONFIG = 2U,
+	CPC_SMC_EVENT_READ_CONFIG = 3U,
+	NF_CPC_SMC_EVENT = 4U,
+};
+
+enum {
+	CPC_SMC_CONFIG_PROF = 0U,
+	CPC_SMC_CONFIG_AUTO_OFF = 1U,
+	CPC_SMC_CONFIG_AUTO_OFF_THRES = 2U,
+	CPC_SMC_CONFIG_CNT_CLR = 3U,
+	CPC_SMC_CONFIG_TIME_SYNC = 4U,
+	NF_CPC_SMC_CONFIG = 5U,
+};
+
+#define us_to_ticks(us)		((us) * 13)
+#define ticks_to_us(tick)	((tick) / 13)
+
+int mtk_cpu_pm_cluster_prot_aquire(unsigned int cluster);
+void mtk_cpu_pm_cluster_prot_release(unsigned int cluster);
+
+void mtk_cpc_mcusys_off_reflect(void);
+int mtk_cpc_mcusys_off_prepare(void);
+
+void mtk_cpc_core_on_hint_set(unsigned int cpu);
+void mtk_cpc_core_on_hint_clr(unsigned int cpu);
+void mtk_cpc_time_sync(void);
+
+uint64_t mtk_cpc_handler(uint64_t act, uint64_t arg1, uint64_t arg2);
+void mtk_cpc_init(void);
+
+#endif /* MT_CPU_PM_CPC_H */
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_mcdi.c b/plat/mediatek/mt8186/drivers/mcdi/mt_mcdi.c
new file mode 100644
index 0000000..0103612
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_mcdi.c
@@ -0,0 +1,150 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <cdefs.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <mt_mcdi.h>
+
+/* Read/Write */
+#define APMCU_MCUPM_MBOX_AP_READY	U(0)
+#define APMCU_MCUPM_MBOX_RESERVED_1	U(1)
+#define APMCU_MCUPM_MBOX_RESERVED_2	U(2)
+#define APMCU_MCUPM_MBOX_RESERVED_3	U(3)
+#define APMCU_MCUPM_MBOX_PWR_CTRL_EN	U(4)
+#define APMCU_MCUPM_MBOX_L3_CACHE_MODE	U(5)
+#define APMCU_MCUPM_MBOX_BUCK_MODE	U(6)
+#define APMCU_MCUPM_MBOX_ARMPLL_MODE	U(7)
+/* Read only */
+#define APMCU_MCUPM_MBOX_TASK_STA	U(8)
+#define APMCU_MCUPM_MBOX_RESERVED_9	U(9)
+#define APMCU_MCUPM_MBOX_RESERVED_10	U(10)
+#define APMCU_MCUPM_MBOX_RESERVED_11	U(11)
+
+/* CPC mode - Read/Write */
+#define APMCU_MCUPM_MBOX_WAKEUP_CPU	U(12)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_PWR_CTRL_EN */
+#define MCUPM_MCUSYS_CTRL		BIT(0)
+#define MCUPM_BUCK_CTRL			BIT(1)
+#define MCUPM_ARMPLL_CTRL		BIT(2)
+#define MCUPM_CM_CTRL			BIT(3)
+#define MCUPM_PWR_CTRL_MASK		GENMASK(3, 0)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_BUCK_MODE */
+#define MCUPM_BUCK_NORMAL_MODE		U(0) /* default */
+#define MCUPM_BUCK_LP_MODE		U(1)
+#define MCUPM_BUCK_OFF_MODE		U(2)
+#define NF_MCUPM_BUCK_MODE		U(3)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_ARMPLL_MODE */
+#define MCUPM_ARMPLL_ON			U(0) /* default */
+#define MCUPM_ARMPLL_GATING		U(1)
+#define MCUPM_ARMPLL_OFF		U(2)
+#define NF_MCUPM_ARMPLL_MODE		U(3)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_TASK_STA */
+#define MCUPM_TASK_UNINIT		U(0)
+#define MCUPM_TASK_INIT			U(1)
+#define MCUPM_TASK_INIT_FINISH		U(2)
+#define MCUPM_TASK_WAIT			U(3)
+#define MCUPM_TASK_RUN			U(4)
+#define MCUPM_TASK_PAUSE		U(5)
+
+#define SSPM_MBOX_3_BASE		U(0x10420000)
+
+#define MCDI_NOT_INIT			U(0)
+#define MCDI_INIT_1			U(1)
+#define MCDI_INIT_2			U(2)
+#define MCDI_INIT_DONE			U(3)
+
+static int mcdi_init_status __section("tzfw_coherent_mem");
+
+static inline uint32_t mcdi_mbox_read(uint32_t id)
+{
+	return mmio_read_32(SSPM_MBOX_3_BASE + (id << 2));
+}
+
+static inline void mcdi_mbox_write(uint32_t id, uint32_t val)
+{
+	mmio_write_32(SSPM_MBOX_3_BASE + (id << 2), val);
+}
+
+static void mtk_mcupm_pwr_ctrl_setting(uint32_t dev)
+{
+	mcdi_mbox_write(APMCU_MCUPM_MBOX_PWR_CTRL_EN, dev);
+}
+
+static void mtk_set_mcupm_pll_mode(uint32_t mode)
+{
+	if (mode < NF_MCUPM_ARMPLL_MODE) {
+		mcdi_mbox_write(APMCU_MCUPM_MBOX_ARMPLL_MODE, mode);
+	}
+}
+
+static void mtk_set_mcupm_buck_mode(uint32_t mode)
+{
+	if (mode < NF_MCUPM_BUCK_MODE) {
+		mcdi_mbox_write(APMCU_MCUPM_MBOX_BUCK_MODE, mode);
+	}
+}
+
+static int mtk_mcupm_is_ready(void)
+{
+	unsigned int sta = mcdi_mbox_read(APMCU_MCUPM_MBOX_TASK_STA);
+
+	return ((sta == MCUPM_TASK_WAIT) || (sta == MCUPM_TASK_INIT_FINISH));
+}
+
+static int mcdi_init_1(void)
+{
+	unsigned int sta = mcdi_mbox_read(APMCU_MCUPM_MBOX_TASK_STA);
+
+	if (sta != MCUPM_TASK_INIT) {
+		return -1;
+	}
+
+	mtk_set_mcupm_pll_mode(MCUPM_ARMPLL_OFF);
+	mtk_set_mcupm_buck_mode(MCUPM_BUCK_OFF_MODE);
+
+	mtk_mcupm_pwr_ctrl_setting(
+			 MCUPM_MCUSYS_CTRL |
+			 MCUPM_BUCK_CTRL |
+			 MCUPM_ARMPLL_CTRL);
+
+	mcdi_mbox_write(APMCU_MCUPM_MBOX_AP_READY, 1);
+
+	return 0;
+}
+
+static int mcdi_init_2(void)
+{
+	return mtk_mcupm_is_ready() ? 0 : -1;
+}
+
+int mcdi_try_init(void)
+{
+	if (mcdi_init_status == MCDI_INIT_DONE) {
+		return 0;
+	}
+
+	if (mcdi_init_status == MCDI_NOT_INIT) {
+		mcdi_init_status = MCDI_INIT_1;
+	}
+
+	if (mcdi_init_status == MCDI_INIT_1 && mcdi_init_1() == 0) {
+		mcdi_init_status = MCDI_INIT_2;
+	}
+
+	if (mcdi_init_status == MCDI_INIT_2 && mcdi_init_2() == 0) {
+		mcdi_init_status = MCDI_INIT_DONE;
+	}
+
+	INFO("mcdi ready for mcusys-off-idle and system suspend\n");
+
+	return (mcdi_init_status == MCDI_INIT_DONE) ? 0 : mcdi_init_status;
+}
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_mcdi.h b/plat/mediatek/mt8186/drivers/mcdi/mt_mcdi.h
new file mode 100644
index 0000000..0e6444a
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_mcdi.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MT_MCDI_H
+#define MT_MCDI_H
+
+int mcdi_try_init(void);
+
+#endif /* MT_MCDI_H */
diff --git a/plat/mediatek/mt8186/drivers/pmic/pmic.c b/plat/mediatek/mt8186/drivers/pmic/pmic.c
new file mode 100644
index 0000000..4f7ab13
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/pmic/pmic.c
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <pmic.h>
+#include <pmic_wrap_init.h>
+
+uint32_t pmic_get_hwcid(void)
+{
+	uint32_t val = 0;
+
+	pwrap_read(PMIC_RG_HWCID_ADDR, &val);
+
+	return val;
+}
+
+void pmic_power_off(void)
+{
+	pwrap_write(PMIC_PWRHOLD, 0x0);
+}
diff --git a/plat/mediatek/mt8186/drivers/pmic/pmic.h b/plat/mediatek/mt8186/drivers/pmic/pmic.h
new file mode 100644
index 0000000..91ccb19
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/pmic/pmic.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PMIC_H
+#define PMIC_H
+
+#include <stdint.h>
+
+#define PMIC_RG_HWCID_ADDR 0x8
+#define PMIC_PWRHOLD 0xa08
+
+/* external API */
+uint32_t pmic_get_hwcid(void);
+void pmic_power_off(void);
+
+#endif /* PMIC_H */
diff --git a/plat/mediatek/mt8186/drivers/pmic/pmic_wrap_init.h b/plat/mediatek/mt8186/drivers/pmic/pmic_wrap_init.h
new file mode 100644
index 0000000..e837456
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/pmic/pmic_wrap_init.h
@@ -0,0 +1,94 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PMIC_WRAP_INIT_H
+#define PMIC_WRAP_INIT_H
+
+#include <stdint.h>
+
+#include "platform_def.h"
+
+/* external API */
+int32_t pwrap_read(uint32_t adr, uint32_t *rdata);
+int32_t pwrap_write(uint32_t adr, uint32_t wdata);
+
+static struct mt8186_pmic_wrap_regs *const mtk_pwrap = (void *)PMIC_WRAP_BASE;
+
+/* timeout setting */
+enum {
+	TIMEOUT_RESET       = 50,	/* us */
+	TIMEOUT_READ        = 50,	/* us */
+	TIMEOUT_WAIT_IDLE   = 50	/* us */
+};
+
+/* PMIC_WRAP registers */
+struct mt8186_pmic_wrap_regs {
+	uint32_t unused[776];
+	uint32_t wacs2_cmd;
+	uint32_t wacs2_rdata;
+	uint32_t wacs2_vldclr;
+};
+
+enum {
+	RDATA_WACS_RDATA_SHIFT = 0,
+	RDATA_WACS_FSM_SHIFT = 16,
+	RDATA_WACS_REQ_SHIFT = 19,
+	RDATA_SYNC_IDLE_SHIFT = 20,
+	RDATA_INIT_DONE_SHIFT = 22,
+	RDATA_SYS_IDLE_SHIFT = 23,
+};
+
+enum {
+	RDATA_WACS_RDATA_MASK = 0xffff,
+	RDATA_WACS_FSM_MASK = 0x7,
+	RDATA_WACS_REQ_MASK = 0x1,
+	RDATA_SYNC_IDLE_MASK = 0x1,
+	RDATA_INIT_DONE_MASK = 0x1,
+	RDATA_SYS_IDLE_MASK = 0x1,
+};
+
+/* WACS_FSM */
+enum {
+	WACS_FSM_IDLE            = 0x00,
+	WACS_FSM_REQ             = 0x02,
+	WACS_FSM_WFDLE           = 0x04,
+	WACS_FSM_WFVLDCLR        = 0x06,
+	WACS_INIT_DONE           = 0x01,
+	WACS_SYNC_IDLE           = 0x01,
+	WACS_SYNC_BUSY           = 0x00
+};
+
+/* error information flag */
+enum {
+	E_PWR_INVALID_ARG             = 1,
+	E_PWR_INVALID_RW              = 2,
+	E_PWR_INVALID_ADDR            = 3,
+	E_PWR_INVALID_WDAT            = 4,
+	E_PWR_INVALID_OP_MANUAL       = 5,
+	E_PWR_NOT_IDLE_STATE          = 6,
+	E_PWR_NOT_INIT_DONE           = 7,
+	E_PWR_NOT_INIT_DONE_READ      = 8,
+	E_PWR_WAIT_IDLE_TIMEOUT       = 9,
+	E_PWR_WAIT_IDLE_TIMEOUT_READ  = 10,
+	E_PWR_INIT_SIDLY_FAIL         = 11,
+	E_PWR_RESET_TIMEOUT           = 12,
+	E_PWR_TIMEOUT                 = 13,
+	E_PWR_INIT_RESET_SPI          = 20,
+	E_PWR_INIT_SIDLY              = 21,
+	E_PWR_INIT_REG_CLOCK          = 22,
+	E_PWR_INIT_ENABLE_PMIC        = 23,
+	E_PWR_INIT_DIO                = 24,
+	E_PWR_INIT_CIPHER             = 25,
+	E_PWR_INIT_WRITE_TEST         = 26,
+	E_PWR_INIT_ENABLE_CRC         = 27,
+	E_PWR_INIT_ENABLE_DEWRAP      = 28,
+	E_PWR_INIT_ENABLE_EVENT       = 29,
+	E_PWR_READ_TEST_FAIL          = 30,
+	E_PWR_WRITE_TEST_FAIL         = 31,
+	E_PWR_SWITCH_DIO              = 32
+};
+
+#endif /* PMIC_WRAP_INIT_H */
diff --git a/plat/mediatek/mt8186/drivers/rtc/rtc.c b/plat/mediatek/mt8186/drivers/rtc/rtc.c
new file mode 100644
index 0000000..4fcf58e
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/rtc/rtc.c
@@ -0,0 +1,133 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <rtc.h>
+
+static void RTC_Config_Interface(uint32_t addr, uint16_t data,
+			    uint16_t MASK, uint16_t SHIFT)
+{
+	uint16_t pmic_reg = 0;
+
+	pmic_reg = RTC_Read(addr);
+
+	pmic_reg &= ~(MASK << SHIFT);
+	pmic_reg |= (data << SHIFT);
+
+	RTC_Write(addr, pmic_reg);
+}
+
+static void rtc_disable_2sec_reboot(void)
+{
+	uint16_t reboot;
+
+	reboot = (RTC_Read(RTC_AL_SEC) & ~RTC_BBPU_2SEC_EN) &
+		 ~RTC_BBPU_AUTO_PDN_SEL;
+	RTC_Write(RTC_AL_SEC, reboot);
+	RTC_Write_Trigger();
+}
+
+static void rtc_xosc_write(uint16_t val, bool reload)
+{
+	uint16_t bbpu;
+
+	RTC_Write(RTC_OSC32CON, RTC_OSC32CON_UNLOCK1);
+	rtc_busy_wait();
+	RTC_Write(RTC_OSC32CON, RTC_OSC32CON_UNLOCK2);
+	rtc_busy_wait();
+
+	RTC_Write(RTC_OSC32CON, val);
+	rtc_busy_wait();
+
+	if (reload) {
+		bbpu = RTC_Read(RTC_BBPU) | RTC_BBPU_KEY | RTC_BBPU_RELOAD;
+		RTC_Write(RTC_BBPU, bbpu);
+		RTC_Write_Trigger();
+	}
+}
+
+static void rtc_enable_k_eosc(void)
+{
+	uint16_t osc32;
+	uint16_t rtc_eosc_cali_td = 8; /* eosc cali period time */
+
+	/* Truning on eosc cali mode clock */
+	RTC_Config_Interface(PMIC_RG_TOP_CON, 1,
+			PMIC_RG_SRCLKEN_IN0_HW_MODE_MASK,
+			PMIC_RG_SRCLKEN_IN0_HW_MODE_SHIFT);
+	RTC_Config_Interface(PMIC_RG_TOP_CON, 1,
+			PMIC_RG_SRCLKEN_IN1_HW_MODE_MASK,
+			PMIC_RG_SRCLKEN_IN1_HW_MODE_SHIFT);
+	RTC_Config_Interface(PMIC_RG_SCK_TOP_CKPDN_CON0, 0,
+			PMIC_RG_RTC_EOSC32_CK_PDN_MASK,
+			PMIC_RG_RTC_EOSC32_CK_PDN_SHIFT);
+
+	switch (rtc_eosc_cali_td) {
+	case 1:
+		RTC_Config_Interface(PMIC_RG_EOSC_CALI_CON0, 0x3,
+			PMIC_RG_EOSC_CALI_TD_MASK, PMIC_RG_EOSC_CALI_TD_SHIFT);
+		break;
+	case 2:
+		RTC_Config_Interface(PMIC_RG_EOSC_CALI_CON0, 0x4,
+			PMIC_RG_EOSC_CALI_TD_MASK, PMIC_RG_EOSC_CALI_TD_SHIFT);
+		break;
+	case 4:
+		RTC_Config_Interface(PMIC_RG_EOSC_CALI_CON0, 0x5,
+			PMIC_RG_EOSC_CALI_TD_MASK, PMIC_RG_EOSC_CALI_TD_SHIFT);
+		break;
+	case 16:
+		RTC_Config_Interface(PMIC_RG_EOSC_CALI_CON0, 0x7,
+			PMIC_RG_EOSC_CALI_TD_MASK, PMIC_RG_EOSC_CALI_TD_SHIFT);
+		break;
+	default:
+		RTC_Config_Interface(PMIC_RG_EOSC_CALI_CON0, 0x6,
+			PMIC_RG_EOSC_CALI_TD_MASK, PMIC_RG_EOSC_CALI_TD_SHIFT);
+		break;
+	}
+	/* Switch the DCXO from 32k-less mode to RTC mode,
+	 * otherwise, EOSC cali will fail
+	 */
+	/* RTC mode will have only OFF mode and FPM */
+	RTC_Config_Interface(PMIC_RG_DCXO_CW02, 0, PMIC_RG_XO_EN32K_MAN_MASK,
+		PMIC_RG_XO_EN32K_MAN_SHIFT);
+	RTC_Write(RTC_BBPU,
+		  RTC_Read(RTC_BBPU) | RTC_BBPU_KEY | RTC_BBPU_RELOAD);
+	RTC_Write_Trigger();
+	/* Enable K EOSC mode for normal power off and then plug out battery */
+	RTC_Write(RTC_AL_YEA, ((RTC_Read(RTC_AL_YEA) | RTC_K_EOSC_RSV_0)
+				& (~RTC_K_EOSC_RSV_1)) | RTC_K_EOSC_RSV_2);
+	RTC_Write_Trigger();
+
+	osc32 = RTC_Read(RTC_OSC32CON);
+	rtc_xosc_write(osc32 | RTC_EMBCK_SRC_SEL, true);
+	INFO("[RTC] RTC_enable_k_eosc\n");
+}
+
+void rtc_power_off_sequence(void)
+{
+	uint16_t bbpu;
+
+	rtc_disable_2sec_reboot();
+	rtc_enable_k_eosc();
+
+	/* clear alarm */
+	bbpu = RTC_BBPU_KEY | RTC_BBPU_CLR | RTC_BBPU_PWREN;
+	if (Writeif_unlock()) {
+		RTC_Write(RTC_BBPU, bbpu);
+
+		RTC_Write(RTC_AL_MASK, RTC_AL_MASK_DOW);
+		RTC_Write_Trigger();
+		mdelay(1);
+
+		bbpu = RTC_Read(RTC_BBPU) | RTC_BBPU_KEY | RTC_BBPU_RELOAD;
+		RTC_Write(RTC_BBPU, bbpu);
+		RTC_Write_Trigger();
+		INFO("[RTC] BBPU=0x%x, IRQ_EN=0x%x, AL_MSK=0x%x, AL_SEC=0x%x\n",
+		     RTC_Read(RTC_BBPU), RTC_Read(RTC_IRQ_EN),
+		     RTC_Read(RTC_AL_MASK), RTC_Read(RTC_AL_SEC));
+	}
+}
diff --git a/plat/mediatek/mt8186/drivers/rtc/rtc.h b/plat/mediatek/mt8186/drivers/rtc/rtc.h
new file mode 100644
index 0000000..b48dbb9
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/rtc/rtc.h
@@ -0,0 +1,145 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef RTC_H
+#define RTC_H
+
+#define PMIC_RG_SRCLKEN_IN0_HW_MODE_MASK	(1U)
+#define PMIC_RG_SRCLKEN_IN0_HW_MODE_SHIFT	(1U)
+#define PMIC_RG_SRCLKEN_IN1_HW_MODE_MASK	(1U)
+#define PMIC_RG_SRCLKEN_IN1_HW_MODE_SHIFT	(3U)
+#define PMIC_RG_RTC_EOSC32_CK_PDN_MASK		(1U)
+#define PMIC_RG_RTC_EOSC32_CK_PDN_SHIFT		(2U)
+#define PMIC_RG_EOSC_CALI_TD_MASK		(7U)
+#define PMIC_RG_EOSC_CALI_TD_SHIFT		(5U)
+#define PMIC_RG_XO_EN32K_MAN_MASK		(1U)
+#define PMIC_RG_XO_EN32K_MAN_SHIFT		(0U)
+
+/* RTC registers */
+enum {
+	RTC_BBPU = 0x0588,
+	RTC_IRQ_STA = 0x058A,
+	RTC_IRQ_EN = 0x058C,
+	RTC_CII_EN = 0x058E
+};
+
+enum {
+	RTC_AL_SEC = 0x05A0,
+	RTC_AL_MIN = 0x05A2,
+	RTC_AL_HOU = 0x05A4,
+	RTC_AL_DOM = 0x05A6,
+	RTC_AL_DOW = 0x05A8,
+	RTC_AL_MTH = 0x05AA,
+	RTC_AL_YEA = 0x05AC,
+	RTC_AL_MASK = 0x0590
+};
+
+enum {
+	RTC_OSC32CON = 0x05AE,
+	RTC_CON = 0x05C4,
+	RTC_WRTGR = 0x05C2
+};
+
+enum {
+	RTC_PDN1 = 0x05B4,
+	RTC_PDN2 = 0x05B6,
+	RTC_SPAR0 = 0x05B8,
+	RTC_SPAR1 = 0x05BA,
+	RTC_PROT = 0x05BC,
+	RTC_DIFF = 0x05BE,
+	RTC_CALI = 0x05C0
+};
+
+enum {
+	RTC_OSC32CON_UNLOCK1 = 0x1A57,
+	RTC_OSC32CON_UNLOCK2 = 0x2B68
+};
+
+enum {
+	RTC_PROT_UNLOCK1 = 0x586A,
+	RTC_PROT_UNLOCK2 = 0x9136
+};
+
+enum {
+	RTC_BBPU_PWREN	= 1U << 0,
+	RTC_BBPU_CLR	= 1U << 1,
+	RTC_BBPU_INIT	= 1U << 2,
+	RTC_BBPU_AUTO	= 1U << 3,
+	RTC_BBPU_CLRPKY	= 1U << 4,
+	RTC_BBPU_RELOAD	= 1U << 5,
+	RTC_BBPU_CBUSY	= 1U << 6
+};
+
+enum {
+	RTC_AL_MASK_SEC = 1U << 0,
+	RTC_AL_MASK_MIN = 1U << 1,
+	RTC_AL_MASK_HOU = 1U << 2,
+	RTC_AL_MASK_DOM = 1U << 3,
+	RTC_AL_MASK_DOW = 1U << 4,
+	RTC_AL_MASK_MTH = 1U << 5,
+	RTC_AL_MASK_YEA = 1U << 6
+};
+
+enum {
+	RTC_BBPU_AUTO_PDN_SEL = 1U << 6,
+	RTC_BBPU_2SEC_CK_SEL = 1U << 7,
+	RTC_BBPU_2SEC_EN = 1U << 8,
+	RTC_BBPU_2SEC_MODE = 0x3 << 9,
+	RTC_BBPU_2SEC_STAT_CLEAR = 1U << 11,
+	RTC_BBPU_2SEC_STAT_STA = 1U << 12
+};
+
+enum {
+	RTC_BBPU_KEY	= 0x43 << 8
+};
+
+enum {
+	RTC_EMBCK_SRC_SEL	= 1 << 8,
+	RTC_EMBCK_SEL_MODE	= 3 << 6,
+	RTC_XOSC32_ENB		= 1 << 5,
+	RTC_REG_XOSC32_ENB	= 1 << 15
+};
+
+enum {
+	RTC_K_EOSC_RSV_0	= 1 << 8,
+	RTC_K_EOSC_RSV_1	= 1 << 9,
+	RTC_K_EOSC_RSV_2	= 1 << 10
+};
+
+/* PMIC TOP Register Definition */
+enum {
+	PMIC_RG_TOP_CON = 0x001E,
+	PMIC_RG_TOP_CKPDN_CON1 = 0x0112,
+	PMIC_RG_TOP_CKPDN_CON1_SET = 0x0114,
+	PMIC_RG_TOP_CKPDN_CON1_CLR = 0x0116,
+	PMIC_RG_TOP_CKSEL_CON0 = 0x0118,
+	PMIC_RG_TOP_CKSEL_CON0_SET = 0x011A,
+	PMIC_RG_TOP_CKSEL_CON0_CLR = 0x011C
+};
+
+/* PMIC SCK Register Definition */
+enum {
+	PMIC_RG_SCK_TOP_CKPDN_CON0 = 0x051A,
+	PMIC_RG_SCK_TOP_CKPDN_CON0_SET = 0x051C,
+	PMIC_RG_SCK_TOP_CKPDN_CON0_CLR = 0x051E,
+	PMIC_RG_EOSC_CALI_CON0 = 0x540
+};
+
+/* PMIC DCXO Register Definition */
+enum {
+	PMIC_RG_DCXO_CW00 = 0x0788,
+	PMIC_RG_DCXO_CW02 = 0x0790
+};
+
+/* external API */
+uint16_t RTC_Read(uint32_t addr);
+void RTC_Write(uint32_t addr, uint16_t data);
+int32_t rtc_busy_wait(void);
+int32_t RTC_Write_Trigger(void);
+int32_t Writeif_unlock(void);
+void rtc_power_off_sequence(void);
+
+#endif /* RTC_H */
diff --git a/plat/mediatek/mt8186/drivers/spmc/mtspmc.c b/plat/mediatek/mt8186/drivers/spmc/mtspmc.c
new file mode 100644
index 0000000..91ef096
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/spmc/mtspmc.c
@@ -0,0 +1,172 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+
+#include <mcucfg.h>
+#include <mtspmc.h>
+#include <mtspmc_private.h>
+#include <plat/common/platform.h>
+
+void mcucfg_disable_gic_wakeup(unsigned int cluster, unsigned int cpu)
+{
+	mmio_setbits_32(MCUCFG_CPC_FLOW_CTRL_CFG, GIC_WAKEUP_IGNORE(cpu));
+}
+
+void mcucfg_enable_gic_wakeup(unsigned int cluster, unsigned int cpu)
+{
+	mmio_clrbits_32(MCUCFG_CPC_FLOW_CTRL_CFG, GIC_WAKEUP_IGNORE(cpu));
+	/* Clear cpu's cpc sw hint */
+	mmio_write_32(CPC_MCUSYS_CPU_ON_SW_HINT_CLR, BIT(cpu));
+}
+
+void mcucfg_set_bootaddr(unsigned int cluster, unsigned int cpu, uintptr_t bootaddr)
+{
+	assert(cluster == 0U);
+
+	mmio_write_32(per_cpu(cluster, cpu, MCUCFG_BOOTADDR), bootaddr);
+}
+
+uintptr_t mcucfg_get_bootaddr(unsigned int cluster, unsigned int cpu)
+{
+	assert(cluster == 0U);
+
+	return (uintptr_t)mmio_read_32(per_cpu(cluster, cpu, MCUCFG_BOOTADDR));
+}
+
+void mcucfg_init_archstate(unsigned int cluster, unsigned int cpu, bool arm64)
+{
+	uint32_t reg;
+
+	assert(cluster == 0U);
+
+	reg = per_cluster(cluster, MCUCFG_INITARCH);
+
+	if (arm64) {
+		mmio_setbits_32(reg, MCUCFG_INITARCH_CPU_BIT(cpu));
+	} else {
+		mmio_clrbits_32(reg, MCUCFG_INITARCH_CPU_BIT(cpu));
+	}
+}
+
+/*
+ * Return subsystem's power state.
+ *
+ * @mask: mask to SPM_CPU_PWR_STATUS to query the power state
+ *        of one subsystem.
+ * RETURNS:
+ * 0 (the subsys was powered off)
+ * 1 (the subsys was powered on)
+ */
+bool spm_get_powerstate(uint32_t mask)
+{
+	return (mmio_read_32(SPM_CPU_PWR_STATUS) & mask);
+}
+
+bool spm_get_cluster_powerstate(unsigned int cluster)
+{
+	assert(cluster == 0U);
+
+	return spm_get_powerstate(MP0_CPUTOP);
+}
+
+bool spm_get_cpu_powerstate(unsigned int cluster, unsigned int cpu)
+{
+	uint32_t mask = BIT(cpu);
+
+	assert(cluster == 0U);
+
+	return spm_get_powerstate(mask);
+}
+
+int spmc_init(void)
+{
+	unsigned int cpu = plat_my_core_pos();
+
+
+	INFO("SPM: enable CPC mode\n");
+
+	mmio_write_32(SPM_POWERON_CONFIG_EN, PROJECT_CODE | BCLK_CG_EN);
+
+	mmio_setbits_32(per_cpu(0, 1, SPM_CPU_PWR), PWR_RST_B);
+	mmio_setbits_32(per_cpu(0, 2, SPM_CPU_PWR), PWR_RST_B);
+	mmio_setbits_32(per_cpu(0, 3, SPM_CPU_PWR), PWR_RST_B);
+	mmio_setbits_32(per_cpu(0, 4, SPM_CPU_PWR), PWR_RST_B);
+	mmio_setbits_32(per_cpu(0, 5, SPM_CPU_PWR), PWR_RST_B);
+	mmio_setbits_32(per_cpu(0, 6, SPM_CPU_PWR), PWR_RST_B);
+	mmio_setbits_32(per_cpu(0, 7, SPM_CPU_PWR), PWR_RST_B);
+
+	mmio_clrbits_32(SPM_MCUSYS_PWR_CON, RESETPWRON_CONFIG);
+	mmio_clrbits_32(SPM_MP0_CPUTOP_PWR_CON, RESETPWRON_CONFIG);
+	mmio_clrbits_32(per_cpu(0, 0, SPM_CPU_PWR), RESETPWRON_CONFIG);
+
+	mmio_setbits_32(MCUCFG_CPC_FLOW_CTRL_CFG, CPC_CTRL_ENABLE);
+
+	/* Clear bootup cpu's cpc sw hint */
+	mmio_write_32(CPC_MCUSYS_CPU_ON_SW_HINT_CLR, BIT(cpu));
+
+	return 0;
+}
+
+/*
+ * Power on a core with specified cluster and core index
+ *
+ * @cluster: the cluster ID of the CPU which to be powered on
+ * @cpu: the CPU ID of the CPU which to be powered on
+ */
+void spm_poweron_cpu(unsigned int cluster, unsigned int cpu)
+{
+	/* info CPC that CPU hotplug on */
+	mmio_setbits_32(MCUCFG_CPC_FLOW_CTRL_CFG, SSPM_ALL_PWR_CTRL_EN);
+
+	/* Set mp0_spmc_pwr_on_cpuX = 1 */
+	mmio_setbits_32(per_cpu(cluster, cpu, SPM_CPU_PWR), PWR_ON);
+
+	/* wait for power on ack */
+	while (!spm_get_cpu_powerstate(cluster, cpu))
+		;
+
+	/* info CPC that CPU hotplug off */
+	mmio_clrbits_32(MCUCFG_CPC_FLOW_CTRL_CFG, SSPM_ALL_PWR_CTRL_EN);
+}
+
+/*
+ * Power off a core with specified cluster and core index
+ *
+ * @cluster: the cluster ID of the CPU which to be powered off
+ * @cpu: the CPU ID of the CPU which to be powered off
+ */
+void spm_poweroff_cpu(unsigned int cluster, unsigned int cpu)
+{
+	/* Set mp0_spmc_pwr_on_cpuX = 0 */
+	mmio_clrbits_32(per_cpu(cluster, cpu, SPM_CPU_PWR), PWR_ON);
+}
+
+/*
+ * Power off a cluster with specified index
+ *
+ * @cluster: the cluster index which to be powered off
+ */
+void spm_poweroff_cluster(unsigned int cluster)
+{
+	/* No need to power on/off cluster on single cluster platform */
+	assert(false);
+}
+
+/*
+ * Power on a cluster with specified index
+ *
+ * @cluster: the cluster index which to be powered on
+ */
+void spm_poweron_cluster(unsigned int cluster)
+{
+	/* No need to power on/off cluster on single cluster platform */
+	assert(false);
+}
diff --git a/plat/mediatek/mt8186/drivers/spmc/mtspmc.h b/plat/mediatek/mt8186/drivers/spmc/mtspmc.h
new file mode 100644
index 0000000..768599b
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/spmc/mtspmc.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTSPMC_H
+#define MTSPMC_H
+
+#include <stdint.h>
+
+int spmc_init(void);
+
+void spm_poweron_cpu(unsigned int cluster, unsigned int cpu);
+void spm_poweroff_cpu(unsigned int cluster, unsigned int cpu);
+
+void spm_poweroff_cluster(unsigned int cluster);
+void spm_poweron_cluster(unsigned int cluster);
+
+bool spm_get_cpu_powerstate(unsigned int cluster, unsigned int cpu);
+bool spm_get_cluster_powerstate(unsigned int cluster);
+bool spm_get_powerstate(uint32_t mask);
+
+void mcucfg_init_archstate(unsigned int cluster, unsigned int cpu, bool arm64);
+void mcucfg_set_bootaddr(unsigned int cluster, unsigned int cpu, uintptr_t bootaddr);
+uintptr_t mcucfg_get_bootaddr(unsigned int cluster, unsigned int cpu);
+
+void mcucfg_disable_gic_wakeup(unsigned int cluster, unsigned int cpu);
+void mcucfg_enable_gic_wakeup(unsigned int cluster, unsigned int cpu);
+
+#endif /* MTSPMC_H */
diff --git a/plat/mediatek/mt8186/drivers/spmc/mtspmc_private.h b/plat/mediatek/mt8186/drivers/spmc/mtspmc_private.h
new file mode 100644
index 0000000..472b54c
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/spmc/mtspmc_private.h
@@ -0,0 +1,176 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTSPMC_PRIVATE_H
+#define MTSPMC_PRIVATE_H
+
+#include <lib/utils_def.h>
+#include <platform_def.h>
+
+unsigned long read_cpuectlr(void);
+void write_cpuectlr(unsigned long cpuectlr);
+
+unsigned long read_cpupwrctlr_el1(void);
+void write_cpupwrctlr_el1(unsigned long cpuectlr);
+
+/* per_cpu/cluster helper */
+struct per_cpu_reg {
+	unsigned int cluster_addr;
+	unsigned int cpu_stride;
+};
+
+#define per_cpu(cluster, cpu, reg)	\
+	(reg[cluster].cluster_addr + (cpu << reg[cluster].cpu_stride))
+
+#define per_cluster(cluster, reg)	(reg[cluster].cluster_addr)
+
+#define SPM_REG(ofs)			(uint32_t)(SPM_BASE + (ofs))
+#define MCUCFG_REG(ofs)			(uint32_t)(MCUCFG_BASE + (ofs))
+#define INFRACFG_AO_REG(ofs)		(uint32_t)(INFRACFG_AO_BASE + (ofs))
+
+/* SPMC related registers */
+#define SPM_POWERON_CONFIG_EN		SPM_REG(0x000)
+/* bit-fields of SPM_POWERON_CONFIG_EN */
+#define PROJECT_CODE			(U(0xb16) << 16)
+#define BCLK_CG_EN			BIT(0)
+
+#define SPM_PWR_STATUS			SPM_REG(0x16c)
+#define SPM_PWR_STATUS_2ND		SPM_REG(0x170)
+#define SPM_CPU_PWR_STATUS		SPM_REG(0x174)
+
+/* bit-fields of SPM_PWR_STATUS */
+#define MD				BIT(0)
+#define CONN				BIT(1)
+#define DDRPHY				BIT(2)
+#define DISP				BIT(3)
+#define MFG				BIT(4)
+#define ISP				BIT(5)
+#define INFRA				BIT(6)
+#define VDEC				BIT(7)
+#define MP0_CPUTOP			BIT(8)
+#define MP0_CPU0			BIT(9)
+#define MP0_CPU1			BIT(10)
+#define MP0_CPU2			BIT(11)
+#define MP0_CPU3			BIT(12)
+#define MCUSYS				BIT(14)
+#define MP0_CPU4			BIT(15)
+#define MP0_CPU5			BIT(16)
+#define MP0_CPU6			BIT(17)
+#define MP0_CPU7			BIT(18)
+#define VEN				BIT(21)
+
+/* SPMC related registers */
+#define SPM_MCUSYS_PWR_CON		SPM_REG(0x200)
+#define SPM_MP0_CPUTOP_PWR_CON		SPM_REG(0x204)
+#define SPM_MP0_CPU0_PWR_CON		SPM_REG(0x208)
+#define SPM_MP0_CPU1_PWR_CON		SPM_REG(0x20c)
+#define SPM_MP0_CPU2_PWR_CON		SPM_REG(0x210)
+#define SPM_MP0_CPU3_PWR_CON		SPM_REG(0x214)
+#define SPM_MP0_CPU4_PWR_CON		SPM_REG(0x218)
+#define SPM_MP0_CPU5_PWR_CON		SPM_REG(0x21c)
+#define SPM_MP0_CPU6_PWR_CON		SPM_REG(0x220)
+#define SPM_MP0_CPU7_PWR_CON		SPM_REG(0x224)
+
+/* bit-fields of SPM_*_PWR_CON */
+#define PWR_ON_ACK			BIT(31)
+#define VPROC_EXT_OFF			BIT(7)
+#define DORMANT_EN			BIT(6)
+#define RESETPWRON_CONFIG		BIT(5)
+#define PWR_CLK_DIS			BIT(4)
+#define PWR_ON				BIT(2)
+#define PWR_RST_B			BIT(0)
+
+/* per_cpu registers for SPM_MP0_CPU_PWR_CON */
+static const struct per_cpu_reg SPM_CPU_PWR[] = {
+	{ .cluster_addr = SPM_MP0_CPU0_PWR_CON, .cpu_stride = 2U }
+};
+
+/* per_cluster registers for SPM_MP0_CPUTOP_PWR_CON */
+static const struct per_cpu_reg SPM_CLUSTER_PWR[] = {
+	{ .cluster_addr = SPM_MP0_CPUTOP_PWR_CON, .cpu_stride = 0U }
+};
+
+/* MCUCFG related registers */
+#define MCUCFG_MP0_CLUSTER_CFG5		MCUCFG_REG(0xc8e4)
+/* reset vectors */
+#define MCUCFG_MP0_CLUSTER_CFG8		MCUCFG_REG(0xc900)
+#define MCUCFG_MP0_CLUSTER_CFG10	MCUCFG_REG(0xc908)
+#define MCUCFG_MP0_CLUSTER_CFG12	MCUCFG_REG(0xc910)
+#define MCUCFG_MP0_CLUSTER_CFG14	MCUCFG_REG(0xc918)
+#define MCUCFG_MP0_CLUSTER_CFG16	MCUCFG_REG(0xc920)
+#define MCUCFG_MP0_CLUSTER_CFG18	MCUCFG_REG(0xc928)
+#define MCUCFG_MP0_CLUSTER_CFG20	MCUCFG_REG(0xc930)
+#define MCUCFG_MP0_CLUSTER_CFG22	MCUCFG_REG(0xc938)
+
+/* per_cpu registers for MCUCFG_MP0_CLUSTER_CFG */
+static const struct per_cpu_reg MCUCFG_BOOTADDR[] = {
+	{ .cluster_addr = MCUCFG_MP0_CLUSTER_CFG8, .cpu_stride = 3U }
+};
+
+/* per_cpu registers for MCUCFG_MP0_CLUSTER_CFG5 */
+static const struct per_cpu_reg MCUCFG_INITARCH[] = {
+	{ .cluster_addr = MCUCFG_MP0_CLUSTER_CFG5, .cpu_stride = 0U }
+};
+
+#define MCUCFG_INITARCH_CPU_BIT(cpu)	BIT(16U + cpu)
+/* CPC control */
+#define MCUCFG_CPC_FLOW_CTRL_CFG	MCUCFG_REG(0xa814)
+#define MCUCFG_CPC_SPMC_PWR_STATUS	MCUCFG_REG(0xa840)
+
+/* bit-fields of CPC_FLOW_CTRL_CFG */
+#define CPC_CTRL_ENABLE			BIT(16)
+#define SSPM_ALL_PWR_CTRL_EN		BIT(13) /* for cpu-hotplug */
+#define GIC_WAKEUP_IGNORE(cpu)		BIT(21 + cpu)
+
+/* bit-fields of CPC_SPMC_PWR_STATUS */
+#define CORE_SPMC_PWR_ON_ACK		GENMASK(11, 0)
+
+/* APB module infracfg_ao */
+#define INFRA_TOPAXI_PROTECTEN		INFRACFG_AO_REG(0x0220)
+#define INFRA_TOPAXI_PROTECTEN_STA0	INFRACFG_AO_REG(0x0224)
+#define INFRA_TOPAXI_PROTECTEN_STA1	INFRACFG_AO_REG(0x0228)
+#define INFRA_TOPAXI_PROTECTEN_SET	INFRACFG_AO_REG(0x02a0)
+#define INFRA_TOPAXI_PROTECTEN_CLR	INFRACFG_AO_REG(0x02a4)
+#define INFRA_TOPAXI_PROTECTEN_1	INFRACFG_AO_REG(0x0250)
+#define INFRA_TOPAXI_PROTECTEN_STA0_1	INFRACFG_AO_REG(0x0254)
+#define INFRA_TOPAXI_PROTECTEN_STA1_1	INFRACFG_AO_REG(0x0258)
+#define INFRA_TOPAXI_PROTECTEN_1_SET	INFRACFG_AO_REG(0x02a8)
+#define INFRA_TOPAXI_PROTECTEN_1_CLR	INFRACFG_AO_REG(0x02ac)
+
+/* bit-fields of INFRA_TOPAXI_PROTECTEN */
+#define MP0_SPMC_PROT_STEP1_0_MASK	BIT(12)
+#define MP0_SPMC_PROT_STEP1_1_MASK	(BIT(26) | BIT(12))
+
+/* SPARK */
+#define VOLTAGE_04			U(0x40)
+#define VOLTAGE_05			U(0x60)
+
+#define PTP3_CPU0_SPMC_SW_CFG		MCUCFG_REG(0x200)
+#define CPU0_ILDO_CONTROL5		MCUCFG_REG(0x334)
+#define CPU0_ILDO_CONTROL8		MCUCFG_REG(0x340)
+
+/* bit-fields of CPU0_ILDO_CONTROL5 */
+#define ILDO_RET_VOSEL			GENMASK(7, 0)
+
+/* bit-fields of PTP3_CPU_SPMC_SW_CFG */
+#define SW_SPARK_EN			BIT(0)
+
+/* bit-fields of CPU0_ILDO_CONTROL8 */
+#define ILDO_BYPASS_B			BIT(0)
+
+static const struct per_cpu_reg MCUCFG_SPARK[] = {
+	{ .cluster_addr = PTP3_CPU0_SPMC_SW_CFG, .cpu_stride = 11U }
+};
+
+static const struct per_cpu_reg ILDO_CONTROL5[] = {
+	{ .cluster_addr = CPU0_ILDO_CONTROL5, .cpu_stride = 11U }
+};
+
+static const struct per_cpu_reg ILDO_CONTROL8[] = {
+	{ .cluster_addr = CPU0_ILDO_CONTROL8, .cpu_stride = 11U }
+};
+
+#endif /* MTSPMC_PRIVATE_H */
diff --git a/plat/mediatek/mt8186/include/mcucfg.h b/plat/mediatek/mt8186/include/mcucfg.h
new file mode 100644
index 0000000..78a01a8
--- /dev/null
+++ b/plat/mediatek/mt8186/include/mcucfg.h
@@ -0,0 +1,255 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MCUCFG_H
+#define MCUCFG_H
+
+#ifndef __ASSEMBLER__
+#include <stdint.h>
+#endif /* __ASSEMBLER__ */
+
+#include <platform_def.h>
+
+#define MCUCFG_REG(ofs)			(uint32_t)(MCUCFG_BASE + (ofs))
+
+#define MP2_MISC_CONFIG_BOOT_ADDR_L(cpu) (MCUCFG_REG(0x2290) + ((cpu) * 8))
+#define MP2_MISC_CONFIG_BOOT_ADDR_H(cpu) (MCUCFG_REG(0x2294) + ((cpu) * 8))
+
+#define MP2_CPUCFG			MCUCFG_REG(0x2208)
+
+#define MP2_CPU0_STANDBYWFE		BIT(4)
+#define MP2_CPU1_STANDBYWFE		BIT(5)
+
+#define MP0_CPUTOP_SPMC_CTL		MCUCFG_REG(0x788)
+#define MP1_CPUTOP_SPMC_CTL		MCUCFG_REG(0x78C)
+#define MP1_CPUTOP_SPMC_SRAM_CTL	MCUCFG_REG(0x790)
+
+#define sw_spark_en			BIT(0)
+#define sw_no_wait_for_q_channel	BIT(1)
+#define sw_fsm_override			BIT(2)
+#define sw_logic_pre1_pdb		BIT(3)
+#define sw_logic_pre2_pdb		BIT(4)
+#define sw_logic_pdb			BIT(5)
+#define sw_iso				BIT(6)
+#define sw_sram_sleepb			(U(0x3F) << 7)
+#define sw_sram_isointb			BIT(13)
+#define sw_clk_dis			BIT(14)
+#define sw_ckiso			BIT(15)
+#define sw_pd				(U(0x3F) << 16)
+#define sw_hot_plug_reset		BIT(22)
+#define sw_pwr_on_override_en		BIT(23)
+#define sw_pwr_on			BIT(24)
+#define sw_coq_dis			BIT(25)
+#define logic_pdbo_all_off_ack		BIT(26)
+#define logic_pdbo_all_on_ack		BIT(27)
+#define logic_pre2_pdbo_all_on_ack	BIT(28)
+#define logic_pre1_pdbo_all_on_ack	BIT(29)
+
+
+#define CPUSYSx_CPUx_SPMC_CTL(cluster, cpu) \
+	(MCUCFG_REG(0x1c30) + cluster * 0x2000 + cpu * 4)
+
+#define CPUSYS0_CPU0_SPMC_CTL		MCUCFG_REG(0x1c30)
+#define CPUSYS0_CPU1_SPMC_CTL		MCUCFG_REG(0x1c34)
+#define CPUSYS0_CPU2_SPMC_CTL		MCUCFG_REG(0x1c38)
+#define CPUSYS0_CPU3_SPMC_CTL		MCUCFG_REG(0x1c3C)
+
+#define CPUSYS1_CPU0_SPMC_CTL		MCUCFG_REG(0x3c30)
+#define CPUSYS1_CPU1_SPMC_CTL		MCUCFG_REG(0x3c34)
+#define CPUSYS1_CPU2_SPMC_CTL		MCUCFG_REG(0x3c38)
+#define CPUSYS1_CPU3_SPMC_CTL		MCUCFG_REG(0x3c3C)
+
+#define cpu_sw_spark_en			BIT(0)
+#define cpu_sw_no_wait_for_q_channel	BIT(1)
+#define cpu_sw_fsm_override		BIT(2)
+#define cpu_sw_logic_pre1_pdb		BIT(3)
+#define cpu_sw_logic_pre2_pdb		BIT(4)
+#define cpu_sw_logic_pdb		BIT(5)
+#define cpu_sw_iso			BIT(6)
+#define cpu_sw_sram_sleepb		BIT(7)
+#define cpu_sw_sram_isointb		BIT(8)
+#define cpu_sw_clk_dis			BIT(9)
+#define cpu_sw_ckiso			BIT(10)
+#define cpu_sw_pd			(U(0x1F) << 11)
+#define cpu_sw_hot_plug_reset		BIT(16)
+#define cpu_sw_powr_on_override_en	BIT(17)
+#define cpu_sw_pwr_on			BIT(18)
+#define cpu_spark2ldo_allswoff		BIT(19)
+#define cpu_pdbo_all_on_ack		BIT(20)
+#define cpu_pre2_pdbo_allon_ack		BIT(21)
+#define cpu_pre1_pdbo_allon_ack		BIT(22)
+
+/* CPC related registers */
+#define CPC_MCUSYS_CPC_OFF_THRES		MCUCFG_REG(0xa714)
+#define CPC_MCUSYS_PWR_CTRL			MCUCFG_REG(0xa804)
+#define CPC_MCUSYS_CPC_FLOW_CTRL_CFG		MCUCFG_REG(0xa814)
+#define CPC_MCUSYS_LAST_CORE_REQ		MCUCFG_REG(0xa818)
+#define CPC_MCUSYS_MP_LAST_CORE_RESP		MCUCFG_REG(0xa81c)
+#define CPC_MCUSYS_LAST_CORE_RESP		MCUCFG_REG(0xa824)
+#define CPC_MCUSYS_PWR_ON_MASK			MCUCFG_REG(0xa828)
+#define CPC_MCUSYS_CPU_ON_SW_HINT_SET		MCUCFG_REG(0xa8a8)
+#define CPC_MCUSYS_CPU_ON_SW_HINT_CLR		MCUCFG_REG(0xa8ac)
+#define CPC_MCUSYS_CPC_DBG_SETTING		MCUCFG_REG(0xab00)
+#define CPC_MCUSYS_CPC_KERNEL_TIME_L_BASE	MCUCFG_REG(0xab04)
+#define CPC_MCUSYS_CPC_KERNEL_TIME_H_BASE	MCUCFG_REG(0xab08)
+#define CPC_MCUSYS_CPC_SYSTEM_TIME_L_BASE	MCUCFG_REG(0xab0c)
+#define CPC_MCUSYS_CPC_SYSTEM_TIME_H_BASE	MCUCFG_REG(0xab10)
+#define CPC_MCUSYS_TRACE_SEL			MCUCFG_REG(0xab14)
+#define CPC_MCUSYS_TRACE_DATA			MCUCFG_REG(0xab20)
+#define CPC_MCUSYS_CLUSTER_COUNTER		MCUCFG_REG(0xab70)
+#define CPC_MCUSYS_CLUSTER_COUNTER_CLR		MCUCFG_REG(0xab74)
+#define SPARK2LDO				MCUCFG_REG(0x2700)
+/* APB module mcucfg */
+#define MP0_CA7_CACHE_CONFIG		MCUCFG_REG(0x000)
+#define MP0_AXI_CONFIG			MCUCFG_REG(0x02C)
+#define MP0_MISC_CONFIG0		MCUCFG_REG(0x030)
+#define MP0_MISC_CONFIG1		MCUCFG_REG(0x034)
+#define MP0_MISC_CONFIG2		MCUCFG_REG(0x038)
+#define MP0_MISC_CONFIG_BOOT_ADDR(cpu)	(MP0_MISC_CONFIG2 + ((cpu) * 8))
+#define MP0_MISC_CONFIG3		MCUCFG_REG(0x03C)
+#define MP0_MISC_CONFIG9		MCUCFG_REG(0x054)
+#define MP0_CA7_MISC_CONFIG		MCUCFG_REG(0x064)
+
+#define MP0_RW_RSVD0			MCUCFG_REG(0x06C)
+
+
+#define MP1_CA7_CACHE_CONFIG		MCUCFG_REG(0x200)
+#define MP1_AXI_CONFIG			MCUCFG_REG(0x22C)
+#define MP1_MISC_CONFIG0		MCUCFG_REG(0x230)
+#define MP1_MISC_CONFIG1		MCUCFG_REG(0x234)
+#define MP1_MISC_CONFIG2		MCUCFG_REG(0x238)
+#define MP1_MISC_CONFIG_BOOT_ADDR(cpu)	(MP1_MISC_CONFIG2 + ((cpu) * 8))
+#define MP1_MISC_CONFIG3		MCUCFG_REG(0x23C)
+#define MP1_MISC_CONFIG9		MCUCFG_REG(0x254)
+#define MP1_CA7_MISC_CONFIG		MCUCFG_REG(0x264)
+
+#define CCI_ADB400_DCM_CONFIG		MCUCFG_REG(0x740)
+#define SYNC_DCM_CONFIG			MCUCFG_REG(0x744)
+
+#define MP0_CLUSTER_CFG0		MCUCFG_REG(0xC8D0)
+
+#define MP0_SPMC			MCUCFG_REG(0x788)
+#define MP1_SPMC			MCUCFG_REG(0x78C)
+#define MP2_AXI_CONFIG			MCUCFG_REG(0x220C)
+#define MP2_AXI_CONFIG_ACINACTM		BIT(0)
+#define MP2_AXI_CONFIG_AINACTS		BIT(4)
+
+#define MPx_AXI_CONFIG_ACINACTM			BIT(4)
+#define MPx_AXI_CONFIG_AINACTS			BIT(5)
+#define MPx_CA7_MISC_CONFIG_standbywfil2	BIT(28)
+
+#define MP0_CPU0_STANDBYWFE		BIT(20)
+#define MP0_CPU1_STANDBYWFE		BIT(21)
+#define MP0_CPU2_STANDBYWFE		BIT(22)
+#define MP0_CPU3_STANDBYWFE		BIT(23)
+
+#define MP1_CPU0_STANDBYWFE		BIT(20)
+#define MP1_CPU1_STANDBYWFE		BIT(21)
+#define MP1_CPU2_STANDBYWFE		BIT(22)
+#define MP1_CPU3_STANDBYWFE		BIT(23)
+
+#define CPUSYS0_SPARKVRETCNTRL		MCUCFG_REG(0x1c00)
+#define CPUSYS0_SPARKEN			MCUCFG_REG(0x1c04)
+#define CPUSYS0_AMUXSEL			MCUCFG_REG(0x1c08)
+#define CPUSYS1_SPARKVRETCNTRL		MCUCFG_REG(0x3c00)
+#define CPUSYS1_SPARKEN			MCUCFG_REG(0x3c04)
+#define CPUSYS1_AMUXSEL			MCUCFG_REG(0x3c08)
+
+#define MP2_PWR_RST_CTL			MCUCFG_REG(0x2008)
+#define MP2_PTP3_CPUTOP_SPMC0		MCUCFG_REG(0x22A0)
+#define MP2_PTP3_CPUTOP_SPMC1		MCUCFG_REG(0x22A4)
+
+#define MP2_COQ				MCUCFG_REG(0x22BC)
+#define MP2_COQ_SW_DIS			BIT(0)
+
+#define MP2_CA15M_MON_SEL		MCUCFG_REG(0x2400)
+#define MP2_CA15M_MON_L			MCUCFG_REG(0x2404)
+
+#define CPUSYS2_CPU0_SPMC_CTL		MCUCFG_REG(0x2430)
+#define CPUSYS2_CPU1_SPMC_CTL		MCUCFG_REG(0x2438)
+#define CPUSYS2_CPU0_SPMC_STA		MCUCFG_REG(0x2434)
+#define CPUSYS2_CPU1_SPMC_STA		MCUCFG_REG(0x243C)
+
+#define MP0_CA7L_DBG_PWR_CTRL		MCUCFG_REG(0x068)
+#define MP1_CA7L_DBG_PWR_CTRL		MCUCFG_REG(0x268)
+#define BIG_DBG_PWR_CTRL		MCUCFG_REG(0x75C)
+
+#define MP2_SW_RST_B			BIT(0)
+#define MP2_TOPAON_APB_MASK		BIT(1)
+
+#define B_SW_HOT_PLUG_RESET		BIT(30)
+
+#define B_SW_PD_OFFSET			(18U)
+#define B_SW_PD				(U(0x3f) << B_SW_PD_OFFSET)
+
+#define B_SW_SRAM_SLEEPB_OFFSET		(12U)
+#define B_SW_SRAM_SLEEPB		(U(0x3f) << B_SW_SRAM_SLEEPB_OFFSET)
+
+#define B_SW_SRAM_ISOINTB		BIT(9)
+#define B_SW_ISO			BIT(8)
+#define B_SW_LOGIC_PDB			BIT(7)
+#define B_SW_LOGIC_PRE2_PDB		BIT(6)
+#define B_SW_LOGIC_PRE1_PDB		BIT(5)
+#define B_SW_FSM_OVERRIDE		BIT(4)
+#define B_SW_PWR_ON			BIT(3)
+#define B_SW_PWR_ON_OVERRIDE_EN		BIT(2)
+
+#define B_FSM_STATE_OUT_OFFSET		(6U)
+#define B_FSM_STATE_OUT_MASK		(U(0x1f) << B_FSM_STATE_OUT_OFFSET)
+#define B_SW_LOGIC_PDBO_ALL_OFF_ACK	BIT(5)
+#define B_SW_LOGIC_PDBO_ALL_ON_ACK	BIT(4)
+#define B_SW_LOGIC_PRE2_PDBO_ALL_ON_ACK	BIT(3)
+#define B_SW_LOGIC_PRE1_PDBO_ALL_ON_ACK	BIT(2)
+
+#define B_FSM_OFF			(0U << B_FSM_STATE_OUT_OFFSET)
+#define B_FSM_ON			(1U << B_FSM_STATE_OUT_OFFSET)
+#define B_FSM_RET			(2U << B_FSM_STATE_OUT_OFFSET)
+
+#ifndef __ASSEMBLER__
+/* cpu boot mode */
+enum {
+	MP0_CPUCFG_64BIT_SHIFT = 12U,
+	MP1_CPUCFG_64BIT_SHIFT = 28U,
+	MP0_CPUCFG_64BIT = U(0xf) << MP0_CPUCFG_64BIT_SHIFT,
+	MP1_CPUCFG_64BIT = U(0xf) << MP1_CPUCFG_64BIT_SHIFT
+};
+
+enum {
+	MP1_DIS_RGU0_WAIT_PD_CPUS_L1_ACK_SHIFT = 0U,
+	MP1_DIS_RGU1_WAIT_PD_CPUS_L1_ACK_SHIFT = 4U,
+	MP1_DIS_RGU2_WAIT_PD_CPUS_L1_ACK_SHIFT = 8U,
+	MP1_DIS_RGU3_WAIT_PD_CPUS_L1_ACK_SHIFT = 12U,
+	MP1_DIS_RGU_NOCPU_WAIT_PD_CPUS_L1_ACK_SHIFT = 16U,
+
+	MP1_DIS_RGU0_WAIT_PD_CPUS_L1_ACK =
+		U(0xf) << MP1_DIS_RGU0_WAIT_PD_CPUS_L1_ACK_SHIFT,
+	MP1_DIS_RGU1_WAIT_PD_CPUS_L1_ACK =
+		U(0xf) << MP1_DIS_RGU1_WAIT_PD_CPUS_L1_ACK_SHIFT,
+	MP1_DIS_RGU2_WAIT_PD_CPUS_L1_ACK =
+		U(0xf) << MP1_DIS_RGU2_WAIT_PD_CPUS_L1_ACK_SHIFT,
+	MP1_DIS_RGU3_WAIT_PD_CPUS_L1_ACK =
+		U(0xf) << MP1_DIS_RGU3_WAIT_PD_CPUS_L1_ACK_SHIFT,
+	MP1_DIS_RGU_NOCPU_WAIT_PD_CPUS_L1_ACK =
+		U(0xf) << MP1_DIS_RGU_NOCPU_WAIT_PD_CPUS_L1_ACK_SHIFT
+};
+
+enum {
+	MP1_AINACTS_SHIFT = 4U,
+	MP1_AINACTS = 1U << MP1_AINACTS_SHIFT
+};
+
+enum {
+	MP1_SW_CG_GEN_SHIFT = 12U,
+	MP1_SW_CG_GEN = 1U << MP1_SW_CG_GEN_SHIFT
+};
+
+enum {
+	MP1_L2RSTDISABLE_SHIFT = 14U,
+	MP1_L2RSTDISABLE = 1U << MP1_L2RSTDISABLE_SHIFT
+};
+#endif /* __ASSEMBLER__ */
+
+#endif  /* MCUCFG_H */
diff --git a/plat/mediatek/mt8186/include/plat_mtk_lpm.h b/plat/mediatek/mt8186/include/plat_mtk_lpm.h
new file mode 100644
index 0000000..347f358
--- /dev/null
+++ b/plat/mediatek/mt8186/include/plat_mtk_lpm.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_MTK_LPM_H
+#define PLAT_MTK_LPM_H
+
+#include <lib/psci/psci.h>
+#include <lib/utils_def.h>
+
+#define MT_IRQ_REMAIN_MAX	U(32)
+#define MT_IRQ_REMAIN_CAT_LOG	BIT(31)
+
+struct mt_irqremain {
+	unsigned int count;
+	unsigned int irqs[MT_IRQ_REMAIN_MAX];
+	unsigned int wakeupsrc_cat[MT_IRQ_REMAIN_MAX];
+	unsigned int wakeupsrc[MT_IRQ_REMAIN_MAX];
+};
+
+#define PLAT_RC_STATUS_READY		BIT(0)
+#define PLAT_RC_STATUS_FEATURE_EN	BIT(1)
+#define PLAT_RC_STATUS_UART_NONSLEEP	BIT(31)
+
+struct mt_lpm_tz {
+	int (*pwr_prompt)(unsigned int cpu, const psci_power_state_t *state);
+	int (*pwr_reflect)(unsigned int cpu, const psci_power_state_t *state);
+
+	int (*pwr_cpu_on)(unsigned int cpu, const psci_power_state_t *state);
+	int (*pwr_cpu_dwn)(unsigned int cpu, const psci_power_state_t *state);
+
+	int (*pwr_cluster_on)(unsigned int cpu,
+					const psci_power_state_t *state);
+	int (*pwr_cluster_dwn)(unsigned int cpu,
+					const psci_power_state_t *state);
+
+	int (*pwr_mcusys_on)(unsigned int cpu, const psci_power_state_t *state);
+	int (*pwr_mcusys_on_finished)(unsigned int cpu,
+					const psci_power_state_t *state);
+	int (*pwr_mcusys_dwn)(unsigned int cpu,
+					const psci_power_state_t *state);
+};
+
+const struct mt_lpm_tz *mt_plat_cpu_pm_init(void);
+
+#endif /* PLAT_MTK_LPM_H */
diff --git a/plat/mediatek/mt8186/include/plat_pm.h b/plat/mediatek/mt8186/include/plat_pm.h
new file mode 100644
index 0000000..436db34
--- /dev/null
+++ b/plat/mediatek/mt8186/include/plat_pm.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_PM_H
+#define PLAT_PM_H
+
+#include <lib/utils_def.h>
+
+#define MT_PLAT_PWR_STATE_CPU			U(1)
+#define MT_PLAT_PWR_STATE_CLUSTER		U(2)
+#define MT_PLAT_PWR_STATE_MCUSYS		U(3)
+#define MT_PLAT_PWR_STATE_SUSPEND2IDLE		U(8)
+#define MT_PLAT_PWR_STATE_SYSTEM_SUSPEND	U(9)
+
+#define MTK_LOCAL_STATE_RUN			U(0)
+#define MTK_LOCAL_STATE_RET			U(1)
+#define MTK_LOCAL_STATE_OFF			U(2)
+
+#define MTK_AFFLVL_CPU				U(0)
+#define MTK_AFFLVL_CLUSTER			U(1)
+#define MTK_AFFLVL_MCUSYS			U(2)
+#define MTK_AFFLVL_SYSTEM			U(3)
+
+#define IS_CLUSTER_OFF_STATE(s)					\
+		is_local_state_off(s->pwr_domain_state[MTK_AFFLVL_CLUSTER])
+#define IS_MCUSYS_OFF_STATE(s)					\
+		is_local_state_off(s->pwr_domain_state[MTK_AFFLVL_MCUSYS])
+#define IS_SYSTEM_SUSPEND_STATE(s)				\
+		is_local_state_off(s->pwr_domain_state[MTK_AFFLVL_SYSTEM])
+
+#define IS_PLAT_SUSPEND_ID(stateid)				\
+		((stateid == MT_PLAT_PWR_STATE_SUSPEND2IDLE)	\
+		|| (stateid == MT_PLAT_PWR_STATE_SYSTEM_SUSPEND))
+
+#endif /* PLAT_PM_H */
diff --git a/plat/mediatek/mt8186/include/plat_sip_calls.h b/plat/mediatek/mt8186/include/plat_sip_calls.h
new file mode 100644
index 0000000..598a5b8
--- /dev/null
+++ b/plat/mediatek/mt8186/include/plat_sip_calls.h
@@ -0,0 +1,15 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_SIP_CALLS_H
+#define PLAT_SIP_CALLS_H
+
+/*******************************************************************************
+ * Plat SiP function constants
+ ******************************************************************************/
+#define MTK_PLAT_SIP_NUM_CALLS    0
+
+#endif /* PLAT_SIP_CALLS_H */
diff --git a/plat/mediatek/mt8186/include/platform_def.h b/plat/mediatek/mt8186/include/platform_def.h
index bf1bbef..479a8d4 100644
--- a/plat/mediatek/mt8186/include/platform_def.h
+++ b/plat/mediatek/mt8186/include/platform_def.h
@@ -15,20 +15,52 @@
 
 /* Aggregate of all devices for MMU mapping */
 #define MTK_DEV_RNG0_BASE	IO_PHYS
-#define MTK_DEV_RNG0_SIZE	0x400000
-#define MTK_DEV_RNG1_BASE	(IO_PHYS + 0x1000000)
-#define MTK_DEV_RNG1_SIZE	0xa110000
+#define MTK_DEV_RNG0_SIZE	0x10000000
 #define MTK_DEV_RNG2_BASE	MT_GIC_BASE
 #define MTK_DEV_RNG2_SIZE	0x600000
 
+#define SPM_BASE		(IO_PHYS + 0x00006000)
 
 /*******************************************************************************
+ * GPIO related constants
+ ******************************************************************************/
+#define GPIO_BASE		(IO_PHYS + 0x00005000)
+#define IOCFG_LT_BASE		(IO_PHYS + 0x00002000)
+#define IOCFG_LM_BASE		(IO_PHYS + 0x00002200)
+#define IOCFG_LB_BASE		(IO_PHYS + 0x00002400)
+#define IOCFG_BL_BASE		(IO_PHYS + 0x00002600)
+#define IOCFG_RB_BASE		(IO_PHYS + 0x00002A00)
+#define IOCFG_RT_BASE		(IO_PHYS + 0x00002C00)
+
+/*******************************************************************************
  * UART related constants
  ******************************************************************************/
 #define UART0_BASE			(IO_PHYS + 0x01002000)
 
 #define UART_BAUDRATE			115200
 
+/*******************************************************************************
+ * PWRAP related constants
+ ******************************************************************************/
+#define PMIC_WRAP_BASE			(IO_PHYS + 0x0000D000)
+
+/*******************************************************************************
+ * EMI MPU related constants
+ ******************************************************************************/
+#define EMI_MPU_BASE		(IO_PHYS + 0x0021B000)
+
+/*******************************************************************************
+ * GIC-600 & interrupt handling related constants
+ ******************************************************************************/
+/* Base MTK_platform compatible GIC memory map */
+#define BASE_GICD_BASE			MT_GIC_BASE
+#define MT_GIC_RDIST_BASE		(MT_GIC_BASE + 0x40000)
+
+#define SYS_CIRQ_BASE			(IO_PHYS + 0x204000)
+#define CIRQ_REG_NUM			11
+#define CIRQ_IRQ_NUM			326
+#define CIRQ_SPI_START			64
+#define MD_WDT_IRQ_BIT_ID		107
 /*******************************************************************************
  * System counter frequency related constants
  ******************************************************************************/
diff --git a/plat/mediatek/mt8186/plat_pm.c b/plat/mediatek/mt8186/plat_pm.c
index 61d2cc9..e8d834d 100644
--- a/plat/mediatek/mt8186/plat_pm.c
+++ b/plat/mediatek/mt8186/plat_pm.c
@@ -4,15 +4,397 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+/* common headers */
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/gpio.h>
 #include <lib/psci/psci.h>
 
+/* platform specific headers */
+#include <mt_gic_v3.h>
+#include <mtspmc.h>
+#include <plat/common/platform.h>
+#include <plat_mtk_lpm.h>
+#include <plat_params.h>
+#include <plat_pm.h>
+#include <pmic.h>
+#include <rtc.h>
+
+/*
+ * Cluster state request:
+ * [0] : The CPU requires cluster power down
+ * [1] : The CPU requires cluster power on
+ */
+#define coordinate_cluster(onoff)	write_clusterpwrdn_el1(onoff)
+#define coordinate_cluster_pwron()	coordinate_cluster(1)
+#define coordinate_cluster_pwroff()	coordinate_cluster(0)
+
+/* platform secure entry point */
+static uintptr_t secure_entrypoint;
+/* per-CPU power state */
+static unsigned int plat_power_state[PLATFORM_CORE_COUNT];
+
+/* platform CPU power domain - ops */
+static const struct mt_lpm_tz *plat_mt_pm;
+
+static inline int plat_mt_pm_invoke(int (*func)(unsigned int cpu,
+						const psci_power_state_t *state),
+				    int cpu, const psci_power_state_t *state)
+{
+	int ret = -1;
+
+	if (func != NULL) {
+		ret = func(cpu, state);
+	}
+	return ret;
+}
+
+/*
+ * Common MTK_platform operations to power on/off a
+ * CPU in response to a CPU_ON, CPU_OFF or CPU_SUSPEND request.
+ */
+static void plat_cpu_pwrdwn_common(unsigned int cpu,
+		const psci_power_state_t *state, unsigned int req_pstate)
+{
+	assert(cpu == plat_my_core_pos());
+	assert(plat_mt_pm != NULL);
+
+	(void)plat_mt_pm_invoke(plat_mt_pm->pwr_cpu_dwn, cpu, state);
+
+	if ((psci_get_pstate_pwrlvl(req_pstate) >= MTK_AFFLVL_CLUSTER) ||
+			(req_pstate == 0U)) { /* hotplug off */
+		coordinate_cluster_pwroff();
+	}
+
+	/* Prevent interrupts from spuriously waking up this CPU */
+	mt_gic_rdistif_save();
+	gicv3_cpuif_disable(cpu);
+	gicv3_rdistif_off(cpu);
+}
+
+static void plat_cpu_pwron_common(unsigned int cpu,
+		const psci_power_state_t *state, unsigned int req_pstate)
+{
+	assert(cpu == plat_my_core_pos());
+	assert(plat_mt_pm != NULL);
+
+	(void)plat_mt_pm_invoke(plat_mt_pm->pwr_cpu_on, cpu, state);
+
+	coordinate_cluster_pwron();
+
+	/*
+	 * If mcusys does power down before then restore
+	 * all CPUs' GIC Redistributors
+	 */
+	if (IS_MCUSYS_OFF_STATE(state)) {
+		mt_gic_rdistif_restore_all();
+	} else {
+		gicv3_rdistif_on(cpu);
+		gicv3_cpuif_enable(cpu);
+		mt_gic_rdistif_init();
+		mt_gic_rdistif_restore();
+	}
+}
+
+/*
+ * Common MTK_platform operations to power on/off a
+ * cluster in response to a CPU_ON, CPU_OFF or CPU_SUSPEND request.
+ */
+static void plat_cluster_pwrdwn_common(unsigned int cpu,
+		const psci_power_state_t *state, unsigned int req_pstate)
+{
+	assert(cpu == plat_my_core_pos());
+	assert(plat_mt_pm != NULL);
+
+	if (plat_mt_pm_invoke(plat_mt_pm->pwr_cluster_dwn, cpu, state) != 0) {
+		coordinate_cluster_pwron();
+
+		/*
+		 * TODO:
+		 * Return on fail and add a 'return' here before
+		 * adding any code following the if-block.
+		 */
+	}
+}
+
+static void plat_cluster_pwron_common(unsigned int cpu,
+		const psci_power_state_t *state, unsigned int req_pstate)
+{
+	assert(cpu == plat_my_core_pos());
+	assert(plat_mt_pm != NULL);
+
+	if (plat_mt_pm_invoke(plat_mt_pm->pwr_cluster_on, cpu, state) != 0) {
+		/*
+		 * TODO:
+		 * return on fail and add a 'return' here before
+		 * adding any code following the if-block.
+		 */
+	}
+}
+
+/*
+ * Common MTK_platform operations to power on/off a
+ * mcusys in response to a CPU_ON, CPU_OFF or CPU_SUSPEND request.
+ */
+static void plat_mcusys_pwrdwn_common(unsigned int cpu,
+		const psci_power_state_t *state, unsigned int req_pstate)
+{
+	assert(cpu == plat_my_core_pos());
+	assert(plat_mt_pm != NULL);
+
+	if (plat_mt_pm_invoke(plat_mt_pm->pwr_mcusys_dwn, cpu, state) != 0) {
+		return;		/* return on fail */
+	}
+
+	mt_gic_distif_save();
+	gic_sgi_save_all();
+}
+
+static void plat_mcusys_pwron_common(unsigned int cpu,
+		const psci_power_state_t *state, unsigned int req_pstate)
+{
+	assert(cpu == plat_my_core_pos());
+	assert(plat_mt_pm != NULL);
+
+	if (plat_mt_pm_invoke(plat_mt_pm->pwr_mcusys_on, cpu, state) != 0) {
+		/* return on fail */
+		return;
+	}
+
+	mt_gic_init();
+	mt_gic_distif_restore();
+	gic_sgi_restore_all();
+
+	(void)plat_mt_pm_invoke(plat_mt_pm->pwr_mcusys_on_finished, cpu, state);
+}
+
+/* plat_psci_ops implementation */
+static void plat_cpu_standby(plat_local_state_t cpu_state)
+{
+	uint64_t scr;
+
+	scr = read_scr_el3();
+	write_scr_el3(scr | SCR_IRQ_BIT | SCR_FIQ_BIT);
+
+	isb();
+	dsb();
+	wfi();
+
+	write_scr_el3(scr);
+}
+
+static int plat_power_domain_on(u_register_t mpidr)
+{
+	unsigned int cpu = (unsigned int)plat_core_pos_by_mpidr(mpidr);
+	unsigned int cluster = 0U;
+
+	if (cpu >= PLATFORM_CORE_COUNT) {
+		return PSCI_E_INVALID_PARAMS;
+	}
+
+	if (!spm_get_cluster_powerstate(cluster)) {
+		spm_poweron_cluster(cluster);
+	}
+
+	/* init CPU reset arch as AARCH64 */
+	mcucfg_init_archstate(cluster, cpu, true);
+	mcucfg_set_bootaddr(cluster, cpu, secure_entrypoint);
+	spm_poweron_cpu(cluster, cpu);
+
+	return PSCI_E_SUCCESS;
+}
+
+static void plat_power_domain_on_finish(const psci_power_state_t *state)
+{
+	unsigned long mpidr = read_mpidr_el1();
+	unsigned int cpu = (unsigned int)plat_core_pos_by_mpidr(mpidr);
+
+	assert(cpu < PLATFORM_CORE_COUNT);
+
+	/* Allow IRQs to wakeup this core in IDLE flow */
+	mcucfg_enable_gic_wakeup(0U, cpu);
+
+	if (IS_CLUSTER_OFF_STATE(state)) {
+		plat_cluster_pwron_common(cpu, state, 0U);
+	}
+
+	plat_cpu_pwron_common(cpu, state, 0U);
+}
+
+static void plat_power_domain_off(const psci_power_state_t *state)
+{
+	unsigned long mpidr = read_mpidr_el1();
+	unsigned int cpu = (unsigned int)plat_core_pos_by_mpidr(mpidr);
+
+	assert(cpu < PLATFORM_CORE_COUNT);
+
+	plat_cpu_pwrdwn_common(cpu, state, 0U);
+	spm_poweroff_cpu(0U, cpu);
+
+	/* prevent unintended IRQs from waking up the hot-unplugged core */
+	mcucfg_disable_gic_wakeup(0U, cpu);
+
+	if (IS_CLUSTER_OFF_STATE(state)) {
+		plat_cluster_pwrdwn_common(cpu, state, 0U);
+	}
+}
+
+static void plat_power_domain_suspend(const psci_power_state_t *state)
+{
+	unsigned int cpu = plat_my_core_pos();
+
+	assert(cpu < PLATFORM_CORE_COUNT);
+	assert(plat_mt_pm != NULL);
+
+	(void)plat_mt_pm_invoke(plat_mt_pm->pwr_prompt, cpu, state);
+
+	/* Perform the common CPU specific operations */
+	plat_cpu_pwrdwn_common(cpu, state, plat_power_state[cpu]);
+
+	if (IS_CLUSTER_OFF_STATE(state)) {
+		/* Perform the common cluster specific operations */
+		plat_cluster_pwrdwn_common(cpu, state, plat_power_state[cpu]);
+	}
+
+	if (IS_MCUSYS_OFF_STATE(state)) {
+		/* Perform the common mcusys specific operations */
+		plat_mcusys_pwrdwn_common(cpu, state, plat_power_state[cpu]);
+	}
+}
+
+static void plat_power_domain_suspend_finish(const psci_power_state_t *state)
+{
+	unsigned int cpu = plat_my_core_pos();
+
+	assert(cpu < PLATFORM_CORE_COUNT);
+	assert(plat_mt_pm != NULL);
+
+	if (IS_MCUSYS_OFF_STATE(state)) {
+		/* Perform the common mcusys specific operations */
+		plat_mcusys_pwron_common(cpu, state, plat_power_state[cpu]);
+	}
+
+	if (IS_CLUSTER_OFF_STATE(state)) {
+		/* Perform the common cluster specific operations */
+		plat_cluster_pwron_common(cpu, state, plat_power_state[cpu]);
+	}
+
+	/* Perform the common CPU specific operations */
+	plat_cpu_pwron_common(cpu, state, plat_power_state[cpu]);
+
+	(void)plat_mt_pm_invoke(plat_mt_pm->pwr_reflect, cpu, state);
+}
+
+static int plat_validate_power_state(unsigned int power_state,
+					psci_power_state_t *req_state)
+{
+	unsigned int pstate = psci_get_pstate_type(power_state);
+	unsigned int aff_lvl = psci_get_pstate_pwrlvl(power_state);
+	unsigned int cpu = plat_my_core_pos();
+
+	if (aff_lvl > PLAT_MAX_PWR_LVL) {
+		return PSCI_E_INVALID_PARAMS;
+	}
+
+	if (pstate == PSTATE_TYPE_STANDBY) {
+		req_state->pwr_domain_state[0] = PLAT_MAX_RET_STATE;
+	} else {
+		unsigned int i;
+		unsigned int pstate_id = psci_get_pstate_id(power_state);
+		plat_local_state_t s = MTK_LOCAL_STATE_OFF;
+
+		/* Use pstate_id to be power domain state */
+		if (pstate_id > s) {
+			s = (plat_local_state_t)pstate_id;
+		}
+
+		for (i = 0U; i <= aff_lvl; i++) {
+			req_state->pwr_domain_state[i] = s;
+		}
+	}
+
+	plat_power_state[cpu] = power_state;
+	return PSCI_E_SUCCESS;
+}
+
+static void plat_get_sys_suspend_power_state(psci_power_state_t *req_state)
+{
+	unsigned int lv;
+	unsigned int cpu = plat_my_core_pos();
+
+	for (lv = PSCI_CPU_PWR_LVL; lv <= PLAT_MAX_PWR_LVL; lv++) {
+		req_state->pwr_domain_state[lv] = PLAT_MAX_OFF_STATE;
+	}
+
+	plat_power_state[cpu] =
+			psci_make_powerstate(
+				MT_PLAT_PWR_STATE_SYSTEM_SUSPEND,
+				PSTATE_TYPE_POWERDOWN, PLAT_MAX_PWR_LVL);
+
+	flush_dcache_range((uintptr_t)
+			&plat_power_state[cpu],
+			sizeof(plat_power_state[cpu]));
+}
+
+/*******************************************************************************
+ * MTK handlers to shutdown/reboot the system
+ ******************************************************************************/
+static void __dead2 plat_mtk_system_reset(void)
+{
+	struct bl_aux_gpio_info *gpio_reset = plat_get_mtk_gpio_reset();
+
+	INFO("MTK System Reset\n");
+
+	gpio_set_value(gpio_reset->index, gpio_reset->polarity);
+
+	wfi();
+	ERROR("MTK System Reset: operation not handled.\n");
+	panic();
+}
+
+static void __dead2 plat_mtk_system_off(void)
+{
+	INFO("MTK System Off\n");
+
+	rtc_power_off_sequence();
+	pmic_power_off();
+
+	wfi();
+	ERROR("MTK System Off: operation not handled.\n");
+	panic();
+}
+
 static const plat_psci_ops_t plat_psci_ops = {
+	.cpu_standby			= plat_cpu_standby,
+	.pwr_domain_on			= plat_power_domain_on,
+	.pwr_domain_on_finish		= plat_power_domain_on_finish,
+	.pwr_domain_off			= plat_power_domain_off,
+	.pwr_domain_suspend		= plat_power_domain_suspend,
+	.pwr_domain_suspend_finish	= plat_power_domain_suspend_finish,
+	.validate_power_state		= plat_validate_power_state,
+	.get_sys_suspend_power_state	= plat_get_sys_suspend_power_state,
+	.system_off			= plat_mtk_system_off,
+	.system_reset			= plat_mtk_system_reset,
 };
 
 int plat_setup_psci_ops(uintptr_t sec_entrypoint,
 			const plat_psci_ops_t **psci_ops)
 {
 	*psci_ops = &plat_psci_ops;
+	secure_entrypoint = sec_entrypoint;
+
+	/*
+	 * init the warm reset config for boot CPU
+	 * reset arch as AARCH64
+	 * reset addr as function bl31_warm_entrypoint()
+	 */
+	mcucfg_init_archstate(0U, 0U, true);
+	mcucfg_set_bootaddr(0U, 0U, secure_entrypoint);
+
+	spmc_init();
+	plat_mt_pm = mt_plat_cpu_pm_init();
 
 	return 0;
 }
diff --git a/plat/mediatek/mt8186/plat_sip_calls.c b/plat/mediatek/mt8186/plat_sip_calls.c
new file mode 100644
index 0000000..716f3d9
--- /dev/null
+++ b/plat/mediatek/mt8186/plat_sip_calls.c
@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+
+uintptr_t mediatek_plat_sip_handler(uint32_t smc_fid,
+				u_register_t x1,
+				u_register_t x2,
+				u_register_t x3,
+				u_register_t x4,
+				void *cookie,
+				void *handle,
+				u_register_t flags)
+{
+	switch (smc_fid) {
+	default:
+		ERROR("%s: unhandled SMC (0x%x)\n", __func__, smc_fid);
+		break;
+	}
+
+	SMC_RET1(handle, SMC_UNK);
+}
diff --git a/plat/mediatek/mt8186/platform.mk b/plat/mediatek/mt8186/platform.mk
index 2d20213..79c6834 100644
--- a/plat/mediatek/mt8186/platform.mk
+++ b/plat/mediatek/mt8186/platform.mk
@@ -8,8 +8,19 @@
 MTK_PLAT_SOC := ${MTK_PLAT}/${PLAT}
 
 PLAT_INCLUDES := -I${MTK_PLAT}/common/                            \
+                 -I${MTK_PLAT}/common/drivers/gic600/             \
+                 -I${MTK_PLAT}/common/drivers/gpio/             \
+                 -I${MTK_PLAT}/common/drivers/timer/              \
+                 -I${MTK_PLAT_SOC}/drivers/dcm/                   \
+                 -I${MTK_PLAT_SOC}/drivers/emi_mpu/               \
+                 -I${MTK_PLAT_SOC}/drivers/gpio/               \
+                 -I${MTK_PLAT_SOC}/drivers/mcdi/                  \
+                 -I${MTK_PLAT_SOC}/drivers/pmic/                  \
+                 -I${MTK_PLAT_SOC}/drivers/rtc/                   \
+                 -I${MTK_PLAT_SOC}/drivers/spmc/                  \
                  -I${MTK_PLAT_SOC}/include/
 
+GICV3_SUPPORT_GIC600        :=      1
 include drivers/arm/gic/v3/gicv3.mk
 include lib/xlat_tables_v2/xlat_tables.mk
 
@@ -20,17 +31,38 @@
 
 
 BL31_SOURCES += common/desc_image_load.c                              \
+                drivers/delay_timer/delay_timer.c                     \
+                drivers/gpio/gpio.c                                   \
+                drivers/delay_timer/generic_delay_timer.c             \
                 drivers/ti/uart/aarch64/16550_console.S               \
                 lib/bl_aux_params/bl_aux_params.c                     \
                 lib/cpus/aarch64/cortex_a55.S                         \
                 lib/cpus/aarch64/cortex_a76.S                         \
                 plat/common/plat_gicv3.c                              \
+                ${MTK_PLAT}/common/drivers/gic600/mt_gic_v3.c         \
+                ${MTK_PLAT}/common/drivers/gpio/mtgpio_common.c       \
+                ${MTK_PLAT}/common/drivers/pmic_wrap/pmic_wrap_init.c \
+                ${MTK_PLAT}/common/drivers/rtc/rtc_common.c           \
                 ${MTK_PLAT}/common/mtk_plat_common.c                  \
+                ${MTK_PLAT}/common/mtk_sip_svc.c                      \
                 ${MTK_PLAT}/common/params_setup.c                     \
+                ${MTK_PLAT}/common/drivers/timer/mt_timer.c           \
+                ${MTK_PLAT}/common/mtk_cirq.c           	      \
                 ${MTK_PLAT_SOC}/aarch64/platform_common.c             \
                 ${MTK_PLAT_SOC}/aarch64/plat_helpers.S                \
                 ${MTK_PLAT_SOC}/bl31_plat_setup.c                     \
+                ${MTK_PLAT_SOC}/drivers/dcm/mtk_dcm.c                 \
+                ${MTK_PLAT_SOC}/drivers/dcm/mtk_dcm_utils.c           \
+                ${MTK_PLAT_SOC}/drivers/emi_mpu/emi_mpu.c             \
+                ${MTK_PLAT_SOC}/drivers/gpio/mtgpio.c                 \
+                ${MTK_PLAT_SOC}/drivers/mcdi/mt_cpu_pm.c              \
+                ${MTK_PLAT_SOC}/drivers/mcdi/mt_cpu_pm_cpc.c          \
+                ${MTK_PLAT_SOC}/drivers/mcdi/mt_mcdi.c                \
+                ${MTK_PLAT_SOC}/drivers/pmic/pmic.c                   \
+                ${MTK_PLAT_SOC}/drivers/rtc/rtc.c                     \
+                ${MTK_PLAT_SOC}/drivers/spmc/mtspmc.c                 \
                 ${MTK_PLAT_SOC}/plat_pm.c                             \
+                ${MTK_PLAT_SOC}/plat_sip_calls.c                      \
                 ${MTK_PLAT_SOC}/plat_topology.c
 
 # Configs for A76 and A55
@@ -40,6 +72,16 @@
 ERRATA_A55_1530923 := 1
 ERRATA_A55_1221012 := 1
 
+ERRATA_A76_1257314 := 1
+ERRATA_A76_1262606 := 1
+ERRATA_A76_1262888 := 1
+ERRATA_A76_1275112 := 1
+ERRATA_A76_1286807 := 1
+ERRATA_A76_1791580 := 1
+ERRATA_A76_1165522 := 1
+ERRATA_A76_1868343 := 1
+ERRATA_A76_1946160 := 1
+
 # indicate the reset vector address can be programmed
 PROGRAMMABLE_RESET_ADDRESS := 1
 
diff --git a/plat/mediatek/mt8195/drivers/spm/mt_spm_internal.c b/plat/mediatek/mt8195/drivers/spm/mt_spm_internal.c
index 2f460e6..2647d9f 100644
--- a/plat/mediatek/mt8195/drivers/spm/mt_spm_internal.c
+++ b/plat/mediatek/mt8195/drivers/spm/mt_spm_internal.c
@@ -24,42 +24,43 @@
 #define ROOT_CORE_ADDR_OFFSET			0x20000000
 #define SPM_WAKEUP_EVENT_MASK_CLEAN_MASK	0xefffffff
 #define	SPM_INIT_DONE_US			20
+#define SPM_WAKEUP_REASON_MISSING		0xdeaddead
 
 static unsigned int mt_spm_bblpm_cnt;
 
 const char *wakeup_src_str[32] = {
-	[0] = "R12_PCM_TIMER",
-	[1] = "R12_RESERVED_DEBUG_B",
-	[2] = "R12_KP_IRQ_B",
-	[3] = "R12_APWDT_EVENT_B",
-	[4] = "R12_APXGPT1_EVENT_B",
-	[5] = "R12_MSDC_WAKEUP_B",
-	[6] = "R12_EINT_EVENT_B",
-	[7] = "R12_IRRX_WAKEUP_B",
-	[8] = "R12_SBD_INTR_WAKEUP_B",
-	[9] = "R12_RESERVE0",
-	[10] = "R12_SC_SSPM2SPM_WAKEUP_B",
-	[11] = "R12_SC_SCP2SPM_WAKEUP_B",
-	[12] = "R12_SC_ADSP2SPM_WAKEUP_B",
-	[13] = "R12_WDT_WAKEUP_B",
-	[14] = "R12_USB_U2_B",
-	[15] = "R12_USB_TOP_B",
-	[16] = "R12_SYS_TIMER_EVENT_B",
-	[17] = "R12_EINT_EVENT_SECURE_B",
-	[18] = "R12_ECE_INT_HDMI_B",
-	[19] = "R12_RESERVE1",
-	[20] = "R12_AFE_IRQ_MCU_B",
-	[21] = "R12_THERM_CTRL_EVENT_B",
-	[22] = "R12_SCP_CIRQ_IRQ_B",
-	[23] = "R12_NNA2INFRA_WAKEUP_B",
-	[24] = "R12_CSYSPWREQ_B",
-	[25] = "R12_RESERVE2",
-	[26] = "R12_PCIE_WAKEUPEVENT_B",
-	[27] = "R12_SEJ_EVENT_B",
-	[28] = "R12_SPM_CPU_WAKEUPEVENT_B",
-	[29] = "R12_APUSYS",
-	[30] = "R12_RESERVE3",
-	[31] = "R12_RESERVE4",
+	[0] = "PCM_TIMER",
+	[1] = "RESERVED_DEBUG_B",
+	[2] = "KEYPAD",
+	[3] = "APWDT",
+	[4] = "APXGPT",
+	[5] = "MSDC",
+	[6] = "EINT",
+	[7] = "IRRX",
+	[8] = "ETHERNET_QOS",
+	[9] = "RESERVE0",
+	[10] = "SSPM",
+	[11] = "SCP",
+	[12] = "ADSP",
+	[13] = "SPM_WDT",
+	[14] = "USB_U2",
+	[15] = "USB_TOP",
+	[16] = "SYS_TIMER",
+	[17] = "EINT_SECURE",
+	[18] = "HDMI",
+	[19] = "RESERVE1",
+	[20] = "AFE",
+	[21] = "THERMAL",
+	[22] = "SYS_CIRQ",
+	[23] = "NNA2INFRA",
+	[24] = "CSYSPWREQ",
+	[25] = "RESERVE2",
+	[26] = "PCIE",
+	[27] = "SEJ",
+	[28] = "SPM_CPU_WAKEUPEVENT",
+	[29] = "APUSYS",
+	[30] = "RESERVE3",
+	[31] = "RESERVE4",
 };
 
 /**************************************
@@ -70,20 +71,26 @@
 				       const struct wake_status *wakesta)
 {
 	uint32_t i, bk_vtcxo_dur, spm_26m_off_pct = 0U;
+	char *spm_26m_sta = NULL;
 	wake_reason_t wr = WR_UNKNOWN;
 
 	if (wakesta == NULL) {
 		return WR_UNKNOWN;
 	}
 
+	spm_26m_sta = ((wakesta->debug_flag & SPM_DBG_DEBUG_IDX_26M_SLEEP) == 0U) ? "on" : "off";
+
 	if (wakesta->abort != 0U) {
-		ERROR("spmfw flow is aborted: 0x%x, timer_out = %u\n",
-		      wakesta->abort, wakesta->timer_out);
+		ERROR("spmfw flow is aborted: 0x%x, timer_out = %u, 26M(%s)\n",
+		      wakesta->abort, wakesta->timer_out, spm_26m_sta);
+	} else if (wakesta->r12 == SPM_WAKEUP_REASON_MISSING) {
+		WARN("cannot find wake up reason, timer_out = %u, 26M(%s)\n",
+		     wakesta->timer_out, spm_26m_sta);
 	} else {
 		for (i = 0U; i < 32U; i++) {
 			if ((wakesta->r12 & (1U << i)) != 0U) {
-				INFO("wake up by %s, timer_out = %u\n",
-				     wakeup_src_str[i], wakesta->timer_out);
+				INFO("wake up by %s, timer_out = %u, 26M(%s)\n",
+				     wakeup_src_str[i], wakesta->timer_out, spm_26m_sta);
 				wr = WR_WAKE_SRC;
 				break;
 			}
diff --git a/plat/rockchip/rk3399/drivers/gpio/rk3399_gpio.c b/plat/rockchip/rk3399/drivers/gpio/rk3399_gpio.c
index b8cba6d..724968f 100644
--- a/plat/rockchip/rk3399/drivers/gpio/rk3399_gpio.c
+++ b/plat/rockchip/rk3399/drivers/gpio/rk3399_gpio.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,15 +18,7 @@
 #include <plat_private.h>
 #include <soc.h>
 
-uint32_t gpio_port[] = {
-	GPIO0_BASE,
-	GPIO1_BASE,
-	GPIO2_BASE,
-	GPIO3_BASE,
-	GPIO4_BASE,
-};
-
-struct {
+struct gpio_save {
 	uint32_t swporta_dr;
 	uint32_t swporta_ddr;
 	uint32_t inten;
@@ -64,97 +56,123 @@
 #define GET_GPIO_BANK(pin)	((pin % 32) / 8)
 #define GET_GPIO_ID(pin)	((pin % 32) % 8)
 
+enum {
+	ENC_ZDZU,
+	ENC_ZUDR,
+	ENC_ZUDZ,
+	NUM_ENC
+};
+
+static const struct port_info {
+	uint32_t clkgate_reg;
+	uint32_t pull_base;
+	uint32_t port_base;
+	/*
+	 * Selects the pull mode encoding per bank,
+	 * first index for pull_type_{hw2sw,sw2hw}
+	 */
+	uint8_t pull_enc[4];
+	uint8_t clkgate_bit;
+	uint8_t max_bank;
+} port_info[] = {
+	{
+		.clkgate_reg = PMUCRU_BASE + CRU_PMU_CLKGATE_CON(1),
+		.pull_base = PMUGRF_BASE + PMUGRF_GPIO0A_P,
+		.port_base = GPIO0_BASE,
+		.pull_enc = {ENC_ZDZU, ENC_ZDZU},
+		.clkgate_bit = PCLK_GPIO0_GATE_SHIFT,
+		.max_bank = 1,
+	}, {
+		.clkgate_reg = PMUCRU_BASE + CRU_PMU_CLKGATE_CON(1),
+		.pull_base = PMUGRF_BASE + PMUGRF_GPIO1A_P,
+		.port_base = GPIO1_BASE,
+		.pull_enc = {ENC_ZUDR, ENC_ZUDR, ENC_ZUDR, ENC_ZUDR},
+		.clkgate_bit = PCLK_GPIO1_GATE_SHIFT,
+		.max_bank = 3,
+	}, {
+		.clkgate_reg = CRU_BASE + CRU_CLKGATE_CON(31),
+		.pull_base = GRF_BASE + GRF_GPIO2A_P,
+		.port_base = GPIO2_BASE,
+		.pull_enc = {ENC_ZUDR, ENC_ZUDR, ENC_ZDZU, ENC_ZDZU},
+		.clkgate_bit = PCLK_GPIO2_GATE_SHIFT,
+		.max_bank = 3,
+	}, {
+		.clkgate_reg = CRU_BASE + CRU_CLKGATE_CON(31),
+		.pull_base = GRF_BASE + GRF_GPIO3A_P,
+		.port_base = GPIO3_BASE,
+		.pull_enc = {ENC_ZUDR, ENC_ZUDR, ENC_ZUDR, ENC_ZUDR},
+		.clkgate_bit = PCLK_GPIO3_GATE_SHIFT,
+		.max_bank = 3,
+	}, {
+		.clkgate_reg = CRU_BASE + CRU_CLKGATE_CON(31),
+		.pull_base = GRF_BASE + GRF_GPIO4A_P,
+		.port_base = GPIO4_BASE,
+		.pull_enc = {ENC_ZUDR, ENC_ZUDR, ENC_ZUDR, ENC_ZUDR},
+		.clkgate_bit = PCLK_GPIO4_GATE_SHIFT,
+		.max_bank = 3,
+	}
+};
+
-/* returns old clock state, enables clock, in order to do GPIO access */
+/*
+ * Mappings between TF-A constants and hardware encodings:
+ * there are 3 different encoding schemes that may differ between
+ * banks of the same port: the corresponding value of the pull_enc array
+ * in port_info is used as the first index
+ */
+static const uint8_t pull_type_hw2sw[NUM_ENC][4] = {
+	[ENC_ZDZU] = {GPIO_PULL_NONE, GPIO_PULL_DOWN, GPIO_PULL_NONE, GPIO_PULL_UP},
+	[ENC_ZUDR] = {GPIO_PULL_NONE, GPIO_PULL_UP, GPIO_PULL_DOWN, GPIO_PULL_REPEATER},
+	[ENC_ZUDZ] = {GPIO_PULL_NONE, GPIO_PULL_UP, GPIO_PULL_DOWN, GPIO_PULL_NONE}
+};
+static const uint8_t pull_type_sw2hw[NUM_ENC][4] = {
+	[ENC_ZDZU] = {
+		[GPIO_PULL_NONE] = 0,
+		[GPIO_PULL_DOWN] = 1,
+		[GPIO_PULL_UP] = 3,
+		[GPIO_PULL_REPEATER] = -1
+	},
+	[ENC_ZUDR] = {
+		[GPIO_PULL_NONE] = 0,
+		[GPIO_PULL_DOWN] = 2,
+		[GPIO_PULL_UP] = 1,
+		[GPIO_PULL_REPEATER] = 3
+	},
+	[ENC_ZUDZ] = {
+		[GPIO_PULL_NONE] = 0,
+		[GPIO_PULL_DOWN] = 2,
+		[GPIO_PULL_UP] = 1,
+		[GPIO_PULL_REPEATER] = -1
+	}
+};
+
+/* Return old clock state, enables clock, in order to do GPIO access */
 static int gpio_get_clock(uint32_t gpio_number)
 {
 	uint32_t port = GET_GPIO_PORT(gpio_number);
-	uint32_t clock_state = 0;
+	assert(port < 5U);
 
-	assert(port < 5);
+	const struct port_info *info = &port_info[port];
 
-	switch (port) {
-	case PMU_GPIO_PORT0:
-		clock_state = (mmio_read_32(PMUCRU_BASE +
-					    CRU_PMU_CLKGATE_CON(1)) >>
-					    PCLK_GPIO0_GATE_SHIFT) & 0x01;
-		mmio_write_32(PMUCRU_BASE + CRU_PMU_CLKGATE_CON(1),
-			      BITS_WITH_WMASK(0, CLK_GATE_MASK,
-					      PCLK_GPIO0_GATE_SHIFT));
-		break;
-	case PMU_GPIO_PORT1:
-		clock_state = (mmio_read_32(PMUCRU_BASE +
-					    CRU_PMU_CLKGATE_CON(1)) >>
-					    PCLK_GPIO1_GATE_SHIFT) & 0x01;
-		mmio_write_32(PMUCRU_BASE + CRU_PMU_CLKGATE_CON(1),
-			      BITS_WITH_WMASK(0, CLK_GATE_MASK,
-					      PCLK_GPIO1_GATE_SHIFT));
-		break;
-	case GPIO_PORT2:
-		clock_state = (mmio_read_32(CRU_BASE +
-					    CRU_CLKGATE_CON(31)) >>
-					    PCLK_GPIO2_GATE_SHIFT) & 0x01;
-		mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
-			      BITS_WITH_WMASK(0, CLK_GATE_MASK,
-					      PCLK_GPIO2_GATE_SHIFT));
-		break;
-	case GPIO_PORT3:
-		clock_state = (mmio_read_32(CRU_BASE +
-					    CRU_CLKGATE_CON(31)) >>
-					    PCLK_GPIO3_GATE_SHIFT) & 0x01;
-		mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
-			      BITS_WITH_WMASK(0, CLK_GATE_MASK,
-					      PCLK_GPIO3_GATE_SHIFT));
-		break;
-	case GPIO_PORT4:
-		clock_state = (mmio_read_32(CRU_BASE +
-					    CRU_CLKGATE_CON(31)) >>
-					    PCLK_GPIO4_GATE_SHIFT) & 0x01;
-		mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
-			      BITS_WITH_WMASK(0, CLK_GATE_MASK,
-					      PCLK_GPIO4_GATE_SHIFT));
-		break;
-	default:
-		break;
+	if ((mmio_read_32(info->clkgate_reg) & (1U << info->clkgate_bit)) == 0U) {
+		return 0;
 	}
-
-	return clock_state;
+	mmio_write_32(
+		info->clkgate_reg,
+		BITS_WITH_WMASK(0, 1, info->clkgate_bit)
+	);
+	return 1;
 }
 
-/* restores old state of gpio clock */
+/* Restore old state of gpio clock, assuming it is running now */
 void gpio_put_clock(uint32_t gpio_number, uint32_t clock_state)
 {
+	if (clock_state == 0) {
+		return;
+	}
 	uint32_t port = GET_GPIO_PORT(gpio_number);
-
-	switch (port) {
-	case PMU_GPIO_PORT0:
-		mmio_write_32(PMUCRU_BASE + CRU_PMU_CLKGATE_CON(1),
-			      BITS_WITH_WMASK(clock_state, CLK_GATE_MASK,
-					      PCLK_GPIO0_GATE_SHIFT));
-		break;
-	case PMU_GPIO_PORT1:
-		mmio_write_32(PMUCRU_BASE + CRU_PMU_CLKGATE_CON(1),
-			      BITS_WITH_WMASK(clock_state, CLK_GATE_MASK,
-					      PCLK_GPIO1_GATE_SHIFT));
-		break;
-	case GPIO_PORT2:
-		mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
-			      BITS_WITH_WMASK(clock_state, CLK_GATE_MASK,
-					      PCLK_GPIO2_GATE_SHIFT));
-		break;
-	case GPIO_PORT3:
-		mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
-			      BITS_WITH_WMASK(clock_state, CLK_GATE_MASK,
-					      PCLK_GPIO3_GATE_SHIFT));
+	const struct port_info *info = &port_info[port];
 
-		break;
-	case GPIO_PORT4:
-		mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
-			      BITS_WITH_WMASK(clock_state, CLK_GATE_MASK,
-					      PCLK_GPIO4_GATE_SHIFT));
-		break;
-	default:
-		break;
-	}
+	mmio_write_32(info->clkgate_reg, BITS_WITH_WMASK(1, 1, info->clkgate_bit));
 }
 
 static int get_pull(int gpio)
@@ -164,39 +182,16 @@
 	uint32_t id = GET_GPIO_ID(gpio);
 	uint32_t val, clock_state;
 
-	assert((port < 5) && (bank < 4));
+	assert(port < 5U);
+	const struct port_info *info = &port_info[port];
 
-	clock_state = gpio_get_clock(gpio);
+	assert(bank <= info->max_bank);
 
-	if (port == PMU_GPIO_PORT0 || port == PMU_GPIO_PORT1) {
-		val = mmio_read_32(PMUGRF_BASE + PMU_GRF_GPIO0A_P +
-				   port * 16 + bank * 4);
-		val = (val >> (id * 2)) & GPIO_P_MASK;
-	} else {
-		val = mmio_read_32(GRF_BASE + GRF_GPIO2A_P +
-				   (port - 2) * 16 + bank * 4);
-		val = (val >> (id * 2)) & GPIO_P_MASK;
-	}
+	clock_state = gpio_get_clock(gpio);
+	val = (mmio_read_32(info->pull_base + 4 * bank) >> (id * 2)) & GPIO_P_MASK;
 	gpio_put_clock(gpio, clock_state);
 
-	/*
-	 * in gpio0a, gpio0b, gpio2c, gpio2d,
-	 * 00: Z
-	 * 01: pull down
-	 * 10: Z
-	 * 11: pull up
-	 * different with other gpio, so need to correct it
-	 */
-	if (((port == 0) && (bank < 2)) || ((port == 2) && (bank > 1))) {
-		if (val == 3)
-			val = GPIO_PULL_UP;
-		else if (val == 1)
-			val = GPIO_PULL_DOWN;
-		else
-			val = 0;
-	}
-
-	return val;
+	return pull_type_hw2sw[info->pull_enc[bank]][val];
 }
 
 static void set_pull(int gpio, int pull)
@@ -206,36 +201,20 @@
 	uint32_t id = GET_GPIO_ID(gpio);
 	uint32_t clock_state;
 
-	assert((port < 5) && (bank < 4));
+	assert(port < 5U);
+	const struct port_info *info = &port_info[port];
 
-	clock_state = gpio_get_clock(gpio);
+	assert(bank <= info->max_bank);
 
-	/*
-	 * in gpio0a, gpio0b, gpio2c, gpio2d,
-	 * 00: Z
-	 * 01: pull down
-	 * 10: Z
-	 * 11: pull up
-	 * different with other gpio, so need to correct it
-	 */
-	if (((port == 0) && (bank < 2)) || ((port == 2) && (bank > 1))) {
-		if (pull == GPIO_PULL_UP)
-			pull = 3;
-		else if (pull == GPIO_PULL_DOWN)
-			pull = 1;
-		else
-			pull = 0;
-	}
+	uint8_t val = pull_type_sw2hw[info->pull_enc[bank]][pull];
 
-	if (port == PMU_GPIO_PORT0 || port == PMU_GPIO_PORT1) {
-		mmio_write_32(PMUGRF_BASE + PMU_GRF_GPIO0A_P +
-			      port * 16 + bank * 4,
-			      BITS_WITH_WMASK(pull, GPIO_P_MASK, id * 2));
-	} else {
-		mmio_write_32(GRF_BASE + GRF_GPIO2A_P +
-			      (port - 2) * 16 + bank * 4,
-			      BITS_WITH_WMASK(pull, GPIO_P_MASK, id * 2));
-	}
+	assert(val != (uint8_t)-1);
+
+	clock_state = gpio_get_clock(gpio);
+	mmio_write_32(
+		info->pull_base + 4 * bank,
+		BITS_WITH_WMASK(val, GPIO_P_MASK, id * 2)
+	);
 	gpio_put_clock(gpio, clock_state);
 }
 
@@ -256,7 +235,10 @@
 	 * but rk3399 gpio direction 1: output, 0: input
 	 * so need to revert direction value
 	 */
-	mmio_setbits_32(gpio_port[port] + SWPORTA_DDR, !direction << num);
+	mmio_setbits_32(
+		port_info[port].port_base + SWPORTA_DDR,
+		((direction == 0) ? 1 : 0) << num
+	);
 	gpio_put_clock(gpio, clock_state);
 }
 
@@ -266,7 +248,7 @@
 	uint32_t num = GET_GPIO_NUM(gpio);
 	int direction, clock_state;
 
-	assert((port < 5) && (num < 32));
+	assert((port < 5U) && (num < 32U));
 
 	clock_state = gpio_get_clock(gpio);
 
@@ -277,8 +259,9 @@
 	 * but rk3399 gpio direction 1: output, 0: input
 	 * so need to revert direction value
 	 */
-	direction = !((mmio_read_32(gpio_port[port] +
-				    SWPORTA_DDR) >> num) & 0x1);
+	direction = (((mmio_read_32(
+		port_info[port].port_base + SWPORTA_DDR
+	) >> num) & 1U) == 0) ? 1 : 0;
 	gpio_put_clock(gpio, clock_state);
 
 	return direction;
@@ -293,7 +276,8 @@
 	assert((port < 5) && (num < 32));
 
 	clock_state = gpio_get_clock(gpio);
-	value = (mmio_read_32(gpio_port[port] + EXT_PORTA) >> num) & 0x1;
+	value = (mmio_read_32(port_info[port].port_base + EXT_PORTA) >> num) &
+		0x1U;
 	gpio_put_clock(gpio, clock_state);
 
 	return value;
@@ -305,17 +289,20 @@
 	uint32_t num = GET_GPIO_NUM(gpio);
 	uint32_t clock_state;
 
-	assert((port < 5) && (num < 32));
+	assert((port < 5U) && (num < 32U));
 
 	clock_state = gpio_get_clock(gpio);
-	mmio_clrsetbits_32(gpio_port[port] + SWPORTA_DR, 1 << num,
-							 !!value << num);
+	mmio_clrsetbits_32(
+		port_info[port].port_base + SWPORTA_DR,
+		1 << num,
+		((value == 0) ? 0 : 1) << num
+	);
 	gpio_put_clock(gpio, clock_state);
 }
 
 void plat_rockchip_save_gpio(void)
 {
-	int i;
+	unsigned int i;
 	uint32_t cru_gate_save;
 
 	cru_gate_save = mmio_read_32(CRU_BASE + CRU_CLKGATE_CON(31));
@@ -335,22 +322,18 @@
 	 * register value
 	 */
 	for (i = 2; i < 5; i++) {
-		store_gpio[i - 2].swporta_dr =
-			mmio_read_32(gpio_port[i] + SWPORTA_DR);
-		store_gpio[i - 2].swporta_ddr =
-			mmio_read_32(gpio_port[i] + SWPORTA_DDR);
-		store_gpio[i - 2].inten =
-			mmio_read_32(gpio_port[i] + INTEN);
-		store_gpio[i - 2].intmask =
-			mmio_read_32(gpio_port[i] + INTMASK);
-		store_gpio[i - 2].inttype_level =
-			mmio_read_32(gpio_port[i] + INTTYPE_LEVEL);
-		store_gpio[i - 2].int_polarity =
-			mmio_read_32(gpio_port[i] + INT_POLARITY);
-		store_gpio[i - 2].debounce =
-			mmio_read_32(gpio_port[i] + DEBOUNCE);
-		store_gpio[i - 2].ls_sync =
-			mmio_read_32(gpio_port[i] + LS_SYNC);
+		uint32_t base = port_info[i].port_base;
+
+		store_gpio[i - 2] = (struct gpio_save) {
+			.swporta_dr = mmio_read_32(base + SWPORTA_DR),
+			.swporta_ddr = mmio_read_32(base + SWPORTA_DDR),
+			.inten = mmio_read_32(base + INTEN),
+			.intmask = mmio_read_32(base + INTMASK),
+			.inttype_level = mmio_read_32(base + INTTYPE_LEVEL),
+			.int_polarity = mmio_read_32(base + INT_POLARITY),
+			.debounce = mmio_read_32(base + DEBOUNCE),
+			.ls_sync = mmio_read_32(base + LS_SYNC),
+		};
 	}
 	mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
 			cru_gate_save | REG_SOC_WMSK);
@@ -386,21 +369,17 @@
 		      BITS_WITH_WMASK(0, 0x07, PCLK_GPIO2_GATE_SHIFT));
 
 	for (i = 2; i < 5; i++) {
-		mmio_write_32(gpio_port[i] + SWPORTA_DR,
-				store_gpio[i - 2].swporta_dr);
-		mmio_write_32(gpio_port[i] + SWPORTA_DDR,
-				store_gpio[i - 2].swporta_ddr);
-		mmio_write_32(gpio_port[i] + INTEN, store_gpio[i - 2].inten);
-		mmio_write_32(gpio_port[i] + INTMASK,
-				store_gpio[i - 2].intmask);
-		mmio_write_32(gpio_port[i] + INTTYPE_LEVEL,
-				store_gpio[i - 2].inttype_level);
-		mmio_write_32(gpio_port[i] + INT_POLARITY,
-				store_gpio[i - 2].int_polarity);
-		mmio_write_32(gpio_port[i] + DEBOUNCE,
-				store_gpio[i - 2].debounce);
-		mmio_write_32(gpio_port[i] + LS_SYNC,
-				store_gpio[i - 2].ls_sync);
+		uint32_t base = port_info[i].port_base;
+		const struct gpio_save *save = &store_gpio[i - 2];
+
+		mmio_write_32(base + SWPORTA_DR, save->swporta_dr);
+		mmio_write_32(base + SWPORTA_DDR, save->swporta_ddr);
+		mmio_write_32(base + INTEN, save->inten);
+		mmio_write_32(base + INTMASK, save->intmask);
+		mmio_write_32(base + INTTYPE_LEVEL, save->inttype_level),
+		mmio_write_32(base + INT_POLARITY, save->int_polarity);
+		mmio_write_32(base + DEBOUNCE, save->debounce);
+		mmio_write_32(base + LS_SYNC, save->ls_sync);
 	}
 	mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
 			cru_gate_save | REG_SOC_WMSK);
diff --git a/plat/rockchip/rk3399/drivers/soc/soc.h b/plat/rockchip/rk3399/drivers/soc/soc.h
index 8539337..8daa5bb 100644
--- a/plat/rockchip/rk3399/drivers/soc/soc.h
+++ b/plat/rockchip/rk3399/drivers/soc/soc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -145,6 +145,8 @@
  * pmugrf reg, offset
  **************************************************/
 #define PMUGRF_OSREG(n)		(0x300 + (n) * 4)
+#define PMUGRF_GPIO0A_P		0x040
+#define PMUGRF_GPIO1A_P		0x050
 
 /**************************************************
  * DCF reg, offset
@@ -248,6 +250,9 @@
 #define GRF_USBPHY1_CTRL16	0x4540
 
 #define GRF_GPIO2A_IOMUX	0xe000
+#define GRF_GPIO2A_P		0xe040
+#define GRF_GPIO3A_P		0xe050
+#define GRF_GPIO4A_P		0xe060
 #define GRF_GPIO2D_HE		0xe18c
 #define GRF_DDRC0_CON0		0xe380
 #define GRF_DDRC0_CON1		0xe384
diff --git a/plat/st/common/include/stm32mp_common.h b/plat/st/common/include/stm32mp_common.h
index fbd0dda..9ca5d16 100644
--- a/plat/st/common/include/stm32mp_common.h
+++ b/plat/st/common/include/stm32mp_common.h
@@ -92,15 +92,6 @@
 /* Print board information */
 void stm32mp_print_boardinfo(void);
 
-/*
- * Util for clock gating and to get clock rate for stm32 and platform drivers
- * @id: Target clock ID, ID used in clock DT bindings
- */
-bool stm32mp_clk_is_enabled(unsigned long id);
-void stm32mp_clk_enable(unsigned long id);
-void stm32mp_clk_disable(unsigned long id);
-unsigned long stm32mp_clk_get_rate(unsigned long id);
-
 /* Initialise the IO layer and register platform IO devices */
 void stm32mp_io_setup(void);
 
diff --git a/plat/st/common/include/stm32mp_dt.h b/plat/st/common/include/stm32mp_dt.h
index f7201c0..a87f941 100644
--- a/plat/st/common/include/stm32mp_dt.h
+++ b/plat/st/common/include/stm32mp_dt.h
@@ -37,6 +37,8 @@
 int dt_match_instance_by_compatible(const char *compatible, uintptr_t address);
 uint32_t dt_get_ddr_size(void);
 uint32_t dt_get_pwr_vdd_voltage(void);
+struct rdev *dt_get_vdd_regulator(void);
+struct rdev *dt_get_cpu_regulator(void);
 const char *dt_get_board_model(void);
 int fdt_get_gpio_bank_pin_count(unsigned int bank);
 
diff --git a/plat/st/common/stm32mp_common.c b/plat/st/common/stm32mp_common.c
index 072155f..918c289 100644
--- a/plat/st/common/stm32mp_common.c
+++ b/plat/st/common/stm32mp_common.c
@@ -9,6 +9,7 @@
 
 #include <arch_helpers.h>
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/st/stm32_console.h>
 #include <drivers/st/stm32mp_clkfunc.h>
@@ -188,13 +189,13 @@
 	}
 #endif
 
-	stm32mp_clk_enable((unsigned long)dt_uart_info.clock);
+	clk_enable((unsigned long)dt_uart_info.clock);
 
 #if defined(IMAGE_BL2)
 	reset_uart((uint32_t)dt_uart_info.reset);
 #endif
 
-	clk_rate = stm32mp_clk_get_rate((unsigned long)dt_uart_info.clock);
+	clk_rate = clk_get_rate((unsigned long)dt_uart_info.clock);
 
 	if (console_stm32_register(dt_uart_info.base, clk_rate,
 				   STM32MP_UART_BAUDRATE, &console) == 0) {
diff --git a/plat/st/common/stm32mp_dt.c b/plat/st/common/stm32mp_dt.c
index 4dc9908..863a90f 100644
--- a/plat/st/common/stm32mp_dt.c
+++ b/plat/st/common/stm32mp_dt.c
@@ -7,16 +7,15 @@
 #include <assert.h>
 #include <errno.h>
 
-#include <libfdt.h>
-
-#include <platform_def.h>
-
 #include <common/debug.h>
 #include <common/fdt_wrappers.h>
+#include <drivers/st/regulator.h>
 #include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stm32mp1_ddr.h>
 #include <drivers/st/stm32mp1_ram.h>
+#include <libfdt.h>
 
+#include <platform_def.h>
 #include <stm32mp_dt.h>
 
 static void *fdt;
@@ -262,37 +261,46 @@
  ******************************************************************************/
 uint32_t dt_get_pwr_vdd_voltage(void)
 {
-	int node, pwr_regulators_node;
-	const fdt32_t *cuint;
+	struct rdev *regul = dt_get_vdd_regulator();
+	uint16_t min;
 
-	node = fdt_node_offset_by_compatible(fdt, -1, DT_PWR_COMPAT);
-	if (node < 0) {
-		INFO("%s: Cannot read PWR node in DT\n", __func__);
+	if (regul == NULL) {
 		return 0;
 	}
 
-	pwr_regulators_node = fdt_subnode_offset(fdt, node, "pwr-regulators");
-	if (pwr_regulators_node < 0) {
-		INFO("%s: Cannot read pwr-regulators node in DT\n", __func__);
-		return 0;
-	}
+	regulator_get_range(regul, &min, NULL);
 
-	cuint = fdt_getprop(fdt, pwr_regulators_node, "vdd-supply", NULL);
-	if (cuint == NULL) {
-		return 0;
-	}
+	return (uint32_t)min * 1000U;
+}
 
-	node = fdt_node_offset_by_phandle(fdt, fdt32_to_cpu(*cuint));
+/*******************************************************************************
+ * This function retrieves VDD supply regulator from DT.
+ * Returns an rdev taken from supply node, NULL otherwise.
+ ******************************************************************************/
+struct rdev *dt_get_vdd_regulator(void)
+{
+	int node = fdt_node_offset_by_compatible(fdt, -1, DT_PWR_COMPAT);
+
 	if (node < 0) {
-		return 0;
+		return NULL;
 	}
 
-	cuint = fdt_getprop(fdt, node, "regulator-min-microvolt", NULL);
-	if (cuint == NULL) {
-		return 0;
+	return regulator_get_by_supply_name(fdt, node, "vdd");
+}
+
+/*******************************************************************************
+ * This function retrieves CPU supply regulator from DT.
+ * Returns an rdev taken from supply node, NULL otherwise.
+ ******************************************************************************/
+struct rdev *dt_get_cpu_regulator(void)
+{
+	int node = fdt_path_offset(fdt, "/cpus/cpu@0");
+
+	if (node < 0) {
+		return NULL;
 	}
 
-	return fdt32_to_cpu(*cuint);
+	return regulator_get_by_supply_name(fdt, node, "cpu");
 }
 
 /*******************************************************************************
diff --git a/plat/st/stm32mp1/bl2_plat_setup.c b/plat/st/stm32mp1/bl2_plat_setup.c
index 512afa8..3c6f48a 100644
--- a/plat/st/stm32mp1/bl2_plat_setup.c
+++ b/plat/st/stm32mp1/bl2_plat_setup.c
@@ -15,6 +15,7 @@
 #include <drivers/generic_delay_timer.h>
 #include <drivers/mmc.h>
 #include <drivers/st/bsec.h>
+#include <drivers/st/regulator_fixed.h>
 #include <drivers/st/stm32_iwdg.h>
 #include <drivers/st/stm32_uart.h>
 #include <drivers/st/stm32mp1_clk.h>
@@ -130,10 +131,6 @@
 {
 	int ret;
 
-	if (dt_pmic_status() > 0) {
-		initialize_pmic();
-	}
-
 	ret = stm32mp1_ddr_probe();
 	if (ret < 0) {
 		ERROR("Invalid DDR init: error %d\n", ret);
@@ -159,7 +156,6 @@
 
 void bl2_el3_plat_arch_setup(void)
 {
-	int32_t result;
 	const char *board_model;
 	boot_api_context_t *boot_context =
 		(boot_api_context_t *)stm32mp_get_boot_ctx_address();
@@ -227,6 +223,16 @@
 	/* Disable MCKPROT */
 	mmio_clrbits_32(rcc_base + RCC_TZCR, RCC_TZCR_MCKPROT);
 
+	/*
+	 * Set minimum reset pulse duration to 31ms for discrete power
+	 * supplied boards.
+	 */
+	if (dt_pmic_status() <= 0) {
+		mmio_clrsetbits_32(rcc_base + RCC_RDLSICR,
+				   RCC_RDLSICR_MRD_MASK,
+				   31U << RCC_RDLSICR_MRD_SHIFT);
+	}
+
 	generic_delay_timer_init();
 
 #if STM32MP_UART_PROGRAMMER
@@ -247,8 +253,6 @@
 		panic();
 	}
 
-	stm32mp1_syscfg_init();
-
 	stm32_save_boot_interface(boot_context->boot_interface_selected,
 				  boot_context->boot_interface_instance);
 
@@ -277,17 +281,23 @@
 	}
 
 skip_console_init:
-	if (stm32_iwdg_init() < 0) {
+	if (fixed_regulator_register() != 0) {
 		panic();
 	}
 
-	stm32_iwdg_refresh();
+	if (dt_pmic_status() > 0) {
+		initialize_pmic();
+		print_pmic_info_and_debug();
+	}
+
+	stm32mp1_syscfg_init();
 
-	result = stm32mp1_dbgmcu_freeze_iwdg2();
-	if (result != 0) {
-		INFO("IWDG2 freeze error : %i\n", result);
+	if (stm32_iwdg_init() < 0) {
+		panic();
 	}
 
+	stm32_iwdg_refresh();
+
 	stm32mp1_auth_ops.check_key = boot_context->bootrom_ecdsa_check_key;
 	stm32mp1_auth_ops.verify_signature =
 		boot_context->bootrom_ecdsa_verify_signature;
diff --git a/plat/st/stm32mp1/include/stm32mp1_dbgmcu.h b/plat/st/stm32mp1/include/stm32mp1_dbgmcu.h
index 498a4f2..3663bce 100644
--- a/plat/st/stm32mp1/include/stm32mp1_dbgmcu.h
+++ b/plat/st/stm32mp1/include/stm32mp1_dbgmcu.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2015-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -13,11 +13,4 @@
 int stm32mp1_dbgmcu_get_chip_version(uint32_t *chip_version);
 int stm32mp1_dbgmcu_get_chip_dev_id(uint32_t *chip_dev_id);
 
-/*
- * Freeze watchdog when a debugger is attached, if the security configuration
- * allows it.
- * Return 0 on success, a negative error value otherwise.
- */
-int stm32mp1_dbgmcu_freeze_iwdg2(void);
-
 #endif /* STM32MP1_DBGMCU_H */
diff --git a/plat/st/stm32mp1/platform.mk b/plat/st/stm32mp1/platform.mk
index 3a76d28..cc1e0d9 100644
--- a/plat/st/stm32mp1/platform.mk
+++ b/plat/st/stm32mp1/platform.mk
@@ -189,6 +189,7 @@
 PLAT_BL_COMMON_SOURCES	+=	lib/cpus/aarch32/cortex_a7.S
 
 PLAT_BL_COMMON_SOURCES	+=	drivers/arm/tzc/tzc400.c				\
+				drivers/clk/clk.c					\
 				drivers/delay_timer/delay_timer.c			\
 				drivers/delay_timer/generic_delay_timer.c		\
 				drivers/st/bsec/bsec.c					\
@@ -200,6 +201,8 @@
 				drivers/st/iwdg/stm32_iwdg.c				\
 				drivers/st/pmic/stm32mp_pmic.c				\
 				drivers/st/pmic/stpmic1.c				\
+				drivers/st/regulator/regulator_core.c			\
+				drivers/st/regulator/regulator_fixed.c			\
 				drivers/st/reset/stm32mp1_reset.c			\
 				plat/st/common/stm32mp_dt.c				\
 				plat/st/stm32mp1/stm32mp1_dbgmcu.c			\
diff --git a/plat/st/stm32mp1/stm32mp1_dbgmcu.c b/plat/st/stm32mp1/stm32mp1_dbgmcu.c
index d026496..1826783 100644
--- a/plat/st/stm32mp1/stm32mp1_dbgmcu.c
+++ b/plat/st/stm32mp1/stm32mp1_dbgmcu.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2016-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <assert.h>
 #include <errno.h>
 
 #include <platform_def.h>
@@ -17,44 +18,32 @@
 #include <stm32mp1_dbgmcu.h>
 
 #define DBGMCU_IDC		U(0x00)
-#define DBGMCU_APB4FZ1		U(0x2C)
 
 #define DBGMCU_IDC_DEV_ID_MASK	GENMASK(11, 0)
 #define DBGMCU_IDC_REV_ID_MASK	GENMASK(31, 16)
 #define DBGMCU_IDC_REV_ID_SHIFT	16
 
-#define DBGMCU_APB4FZ1_IWDG2	BIT(2)
-
-static uintptr_t get_rcc_base(void)
-{
-	/* This is called before stm32mp_rcc_base() is available */
-	return RCC_BASE;
-}
-
 static int stm32mp1_dbgmcu_init(void)
 {
-	uint32_t dbg_conf;
-	uintptr_t rcc_base = get_rcc_base();
-
-	dbg_conf = bsec_read_debug_conf();
-
-	if ((dbg_conf & BSEC_DBGSWGEN) == 0U) {
-		uint32_t result = bsec_write_debug_conf(dbg_conf |
-							BSEC_DBGSWGEN);
-
-		if (result != BSEC_OK) {
-			ERROR("Error enabling DBGSWGEN\n");
-			return -1;
-		}
+	if ((bsec_read_debug_conf() & BSEC_DBGSWGEN) == 0U) {
+		INFO("Software access to all debug components is disabled\n");
+		return -1;
 	}
 
-	mmio_setbits_32(rcc_base + RCC_DBGCFGR, RCC_DBGCFGR_DBGCKEN);
+	mmio_setbits_32(RCC_BASE + RCC_DBGCFGR, RCC_DBGCFGR_DBGCKEN);
 
 	return 0;
 }
 
+/*
+ * @brief  Get silicon revision from DBGMCU registers.
+ * @param  chip_version: pointer to the read value.
+ * @retval 0 on success, negative value on failure.
+ */
 int stm32mp1_dbgmcu_get_chip_version(uint32_t *chip_version)
 {
+	assert(chip_version != NULL);
+
 	if (stm32mp1_dbgmcu_init() != 0) {
 		return -EPERM;
 	}
@@ -65,32 +54,21 @@
 	return 0;
 }
 
+/*
+ * @brief  Get device ID from DBGMCU registers.
+ * @param  chip_dev_id: pointer to the read value.
+ * @retval 0 on success, negative value on failure.
+ */
 int stm32mp1_dbgmcu_get_chip_dev_id(uint32_t *chip_dev_id)
 {
-	if (stm32mp1_dbgmcu_init() != 0) {
-		return -EPERM;
-	}
-
-	*chip_dev_id = mmio_read_32(DBGMCU_BASE + DBGMCU_IDC) &
-		DBGMCU_IDC_DEV_ID_MASK;
-
-	return 0;
-}
-
-int stm32mp1_dbgmcu_freeze_iwdg2(void)
-{
-	uint32_t dbg_conf;
+	assert(chip_dev_id != NULL);
 
 	if (stm32mp1_dbgmcu_init() != 0) {
 		return -EPERM;
 	}
 
-	dbg_conf = bsec_read_debug_conf();
-
-	if ((dbg_conf & (BSEC_SPIDEN | BSEC_SPINDEN)) != 0U) {
-		mmio_setbits_32(DBGMCU_BASE + DBGMCU_APB4FZ1,
-				DBGMCU_APB4FZ1_IWDG2);
-	}
+	*chip_dev_id = mmio_read_32(DBGMCU_BASE + DBGMCU_IDC) &
+		       DBGMCU_IDC_DEV_ID_MASK;
 
 	return 0;
 }
diff --git a/plat/st/stm32mp1/stm32mp1_def.h b/plat/st/stm32mp1/stm32mp1_def.h
index f5d4b2f..b43245f 100644
--- a/plat/st/stm32mp1/stm32mp1_def.h
+++ b/plat/st/stm32mp1/stm32mp1_def.h
@@ -461,6 +461,14 @@
 #define SYSCFG_BASE			U(0x50020000)
 
 /*******************************************************************************
+ * REGULATORS
+ ******************************************************************************/
+/* 3 PWR + 1 VREFBUF + 14 PMIC regulators + 1 FIXED */
+#define PLAT_NB_RDEVS			U(19)
+/* 1 FIXED */
+#define PLAT_NB_FIXED_REGS		U(1)
+
+/*******************************************************************************
  * Device Tree defines
  ******************************************************************************/
 #define DT_BSEC_COMPAT			"st,stm32mp15-bsec"
diff --git a/plat/st/stm32mp1/stm32mp1_fconf_firewall.c b/plat/st/stm32mp1/stm32mp1_fconf_firewall.c
index caf9ff1..a1969eb 100644
--- a/plat/st/stm32mp1/stm32mp1_fconf_firewall.c
+++ b/plat/st/stm32mp1/stm32mp1_fconf_firewall.c
@@ -9,7 +9,7 @@
 #include <common/debug.h>
 #include <common/fdt_wrappers.h>
 #include <drivers/arm/tzc400.h>
-#include <drivers/st/stm32mp1_clk.h>
+#include <drivers/clk.h>
 #include <dt-bindings/clock/stm32mp1-clks.h>
 #include <lib/fconf/fconf.h>
 #include <lib/object_pool.h>
@@ -31,8 +31,8 @@
 
 void stm32mp1_arch_security_setup(void)
 {
-	stm32mp_clk_enable(TZC1);
-	stm32mp_clk_enable(TZC2);
+	clk_enable(TZC1);
+	clk_enable(TZC2);
 
 	tzc400_init(STM32MP1_TZC_BASE);
 	tzc400_disable_filters();
diff --git a/plat/st/stm32mp1/stm32mp1_pm.c b/plat/st/stm32mp1/stm32mp1_pm.c
index 31a9ae7..ed4d5e5 100644
--- a/plat/st/stm32mp1/stm32mp1_pm.c
+++ b/plat/st/stm32mp1/stm32mp1_pm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,7 +14,7 @@
 #include <common/debug.h>
 #include <drivers/arm/gic_common.h>
 #include <drivers/arm/gicv2.h>
-#include <drivers/st/stm32mp1_clk.h>
+#include <drivers/clk.h>
 #include <dt-bindings/clock/stm32mp1-clks.h>
 #include <lib/mmio.h>
 #include <lib/psci/psci.h>
@@ -74,7 +74,7 @@
 		return PSCI_E_INVALID_ADDRESS;
 	}
 
-	stm32mp_clk_enable(RTCAPB);
+	clk_enable(RTCAPB);
 
 	cntfrq_core0 = read_cntfrq_el0();
 
@@ -84,7 +84,7 @@
 	/* Write magic number in backup register */
 	mmio_write_32(bkpr_core1_magic, BOOT_API_A7_CORE1_MAGIC_NUMBER);
 
-	stm32mp_clk_disable(RTCAPB);
+	clk_disable(RTCAPB);
 
 	/* Generate an IT to core 1 */
 	gicv2_raise_sgi(ARM_IRQ_SEC_SGI_0, STM32MP_SECONDARY_CPU);
diff --git a/plat/st/stm32mp1/stm32mp1_private.c b/plat/st/stm32mp1/stm32mp1_private.c
index 50112d0..e58528e 100644
--- a/plat/st/stm32mp1/stm32mp1_private.c
+++ b/plat/st/stm32mp1/stm32mp1_private.c
@@ -6,6 +6,7 @@
 
 #include <assert.h>
 
+#include <drivers/clk.h>
 #include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stm32_iwdg.h>
 #include <lib/mmio.h>
@@ -566,14 +567,14 @@
 {
 	uint32_t bkpr_itf_idx = tamp_bkpr(TAMP_BOOT_MODE_BACKUP_REG_ID);
 
-	stm32mp_clk_enable(RTCAPB);
+	clk_enable(RTCAPB);
 
 	mmio_clrsetbits_32(bkpr_itf_idx,
 			   TAMP_BOOT_MODE_ITF_MASK,
 			   ((interface << 4) | (instance & 0xFU)) <<
 			   TAMP_BOOT_MODE_ITF_SHIFT);
 
-	stm32mp_clk_disable(RTCAPB);
+	clk_disable(RTCAPB);
 }
 
 void stm32_get_boot_interface(uint32_t *interface, uint32_t *instance)
@@ -583,12 +584,12 @@
 	if (itf == 0U) {
 		uint32_t bkpr = tamp_bkpr(TAMP_BOOT_MODE_BACKUP_REG_ID);
 
-		stm32mp_clk_enable(RTCAPB);
+		clk_enable(RTCAPB);
 
 		itf = (mmio_read_32(bkpr) & TAMP_BOOT_MODE_ITF_MASK) >>
 			TAMP_BOOT_MODE_ITF_SHIFT;
 
-		stm32mp_clk_disable(RTCAPB);
+		clk_disable(RTCAPB);
 	}
 
 	*interface = itf >> 4;
diff --git a/plat/st/stm32mp1/stm32mp1_scmi.c b/plat/st/stm32mp1/stm32mp1_scmi.c
index 6d60bd4..98585dc 100644
--- a/plat/st/stm32mp1/stm32mp1_scmi.c
+++ b/plat/st/stm32mp1/stm32mp1_scmi.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2019-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -8,6 +8,7 @@
 
 #include <platform_def.h>
 
+#include <drivers/clk.h>
 #include <drivers/scmi-msg.h>
 #include <drivers/scmi.h>
 #include <drivers/st/stm32mp1_clk.h>
@@ -274,7 +275,7 @@
 	if (array == NULL) {
 		*nb_elts = 1U;
 	} else if (*nb_elts == 1U) {
-		*array = stm32mp_clk_get_rate(clock->clock_id);
+		*array = clk_get_rate(clock->clock_id);
 	} else {
 		return SCMI_GENERIC_ERROR;
 	}
@@ -292,7 +293,7 @@
 		return 0U;
 	}
 
-	return stm32mp_clk_get_rate(clock->clock_id);
+	return clk_get_rate(clock->clock_id);
 }
 
 int32_t plat_scmi_clock_get_state(unsigned int agent_id, unsigned int scmi_id)
@@ -323,13 +324,13 @@
 	if (enable_not_disable) {
 		if (!clock->enabled) {
 			VERBOSE("SCMI clock %u enable\n", scmi_id);
-			stm32mp_clk_enable(clock->clock_id);
+			clk_enable(clock->clock_id);
 			clock->enabled = true;
 		}
 	} else {
 		if (clock->enabled) {
 			VERBOSE("SCMI clock %u disable\n", scmi_id);
-			stm32mp_clk_disable(clock->clock_id);
+			clk_disable(clock->clock_id);
 			clock->enabled = false;
 		}
 	}
@@ -461,7 +462,7 @@
 			/* Sync SCMI clocks with their targeted initial state */
 			if (clk->enabled &&
 			    stm32mp_nsec_can_access_clock(clk->clock_id)) {
-				stm32mp_clk_enable(clk->clock_id);
+				clk_enable(clk->clock_id);
 			}
 		}
 
diff --git a/plat/st/stm32mp1/stm32mp1_security.c b/plat/st/stm32mp1/stm32mp1_security.c
index 19ef4f0..c84bffc 100644
--- a/plat/st/stm32mp1/stm32mp1_security.c
+++ b/plat/st/stm32mp1/stm32mp1_security.c
@@ -10,6 +10,7 @@
 
 #include <common/debug.h>
 #include <drivers/arm/tzc400.h>
+#include <drivers/clk.h>
 #include <drivers/st/stm32mp1_clk.h>
 #include <dt-bindings/clock/stm32mp1-clks.h>
 #include <dt-bindings/soc/stm32mp15-tzc400.h>
@@ -106,8 +107,8 @@
  ******************************************************************************/
 static void early_init_tzc400(void)
 {
-	stm32mp_clk_enable(TZC1);
-	stm32mp_clk_enable(TZC2);
+	clk_enable(TZC1);
+	clk_enable(TZC2);
 
 	/* Region 0 set to cover all DRAM secure at 0xC000_0000 */
 	init_tzc400_begin(TZC_REGION_S_RDWR);
diff --git a/plat/xilinx/versal/bl31_versal_setup.c b/plat/xilinx/versal/bl31_versal_setup.c
index f59859e..60f7449 100644
--- a/plat/xilinx/versal/bl31_versal_setup.c
+++ b/plat/xilinx/versal/bl31_versal_setup.c
@@ -64,7 +64,7 @@
 {
 	uint64_t atf_handoff_addr;
 
-	if (VERSAL_CONSOLE_IS(pl011)) {
+	if (VERSAL_CONSOLE_IS(pl011) || (VERSAL_CONSOLE_IS(pl011_1))) {
 		static console_t versal_runtime_console;
 		/* Initialize the console to provide early debug support */
 		int rc = console_pl011_register((unsigned long)VERSAL_UART_BASE,
diff --git a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
index 47be4e1..58eee3a 100644
--- a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
+++ b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
@@ -68,7 +68,7 @@
 {
 	uint64_t atf_handoff_addr;
 
-	if (ZYNQMP_CONSOLE_IS(cadence)) {
+	if (ZYNQMP_CONSOLE_IS(cadence) || (ZYNQMP_CONSOLE_IS(cadence1))) {
 		/* Register the console to provide early debug support */
 		static console_t bl31_boot_console;
 		(void)console_cdns_register(ZYNQMP_UART_BASE,
diff --git a/services/std_svc/rmmd/trp/trp_entry.S b/services/std_svc/rmmd/trp/trp_entry.S
index 23b48fb..5826d75 100644
--- a/services/std_svc/rmmd/trp/trp_entry.S
+++ b/services/std_svc/rmmd/trp/trp_entry.S
@@ -33,8 +33,19 @@
 	 */
 trp_head:
 	bl	plat_set_my_stack
-	bl	plat_is_my_cpu_primary
-	cbz	x0, trp_secondary_cpu_entry
+
+	/*
+	 * Find out whether this is a cold or warm boot
+	 */
+	ldr	x1, cold_boot_flag
+	cbz	x1, warm_boot
+
+	/*
+	 * Update cold boot flag to indicate cold boot is done
+	 */
+	adr	x2, cold_boot_flag
+	str	xzr, [x2]
+
 
 	/* ---------------------------------------------
 	 * Zero out BSS section
@@ -47,12 +58,20 @@
 	bl	trp_setup
 
 	bl	trp_main
-trp_secondary_cpu_entry:
+warm_boot:
 	mov_imm	x0, RMI_RMM_REQ_COMPLETE
 	mov	x1, xzr
 	smc	#0
 	b	trp_handler
 
+	/*
+	 * Flag to mark if it is a cold boot.
+	 * 1: cold boot, 0: warmboot.
+	 */
+.align 3
+cold_boot_flag:
+	.dword		1
+
 	/* ---------------------------------------------
 	 *   Direct SMC call to BL31 service provided by
 	 *   RMM Dispatcher
diff --git a/tools/fiptool/Makefile b/tools/fiptool/Makefile
index 11d2e7b..7c2a083 100644
--- a/tools/fiptool/Makefile
+++ b/tools/fiptool/Makefile
@@ -12,6 +12,8 @@
 PROJECT := $(notdir ${FIPTOOL})
 OBJECTS := fiptool.o tbbr_config.o
 V ?= 0
+OPENSSL_DIR := /usr
+
 
 override CPPFLAGS += -D_GNU_SOURCE -D_XOPEN_SOURCE=700
 HOSTCCFLAGS := -Wall -Werror -pedantic -std=c99
@@ -20,7 +22,7 @@
 else
   HOSTCCFLAGS += -O2
 endif
-LDLIBS := -lcrypto
+LDLIBS := -L${OPENSSL_DIR}/lib -lcrypto
 
 ifeq (${V},0)
   Q := @
@@ -28,7 +30,7 @@
   Q :=
 endif
 
-INCLUDE_PATHS := -I../../include/tools_share
+INCLUDE_PATHS := -I../../include/tools_share  -I${OPENSSL_DIR}/include
 
 HOSTCC ?= gcc
 
diff --git a/tools/fiptool/fiptool.c b/tools/fiptool/fiptool.c
index d92c31d..5c240b5 100644
--- a/tools/fiptool/fiptool.c
+++ b/tools/fiptool/fiptool.c
@@ -537,7 +537,7 @@
 	for (desc = image_desc_head; desc != NULL; desc = desc->next) {
 		image_t *image = desc->image;
 
-		if (image == NULL)
+		if (image == NULL || (image->toc_e.size == 0ULL))
 			continue;
 		payload_size += image->toc_e.size;
 		entry_offset = (entry_offset + align - 1) & ~(align - 1);