Adding upstream version 2.8.0+dfsg.upstream/2.8.0+dfsgupstream

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

10 files changed, 1263 insertions, 0 deletions

diff --git a/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm.c b/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm.c
new file mode 100644
index 0000000..313ad47
--- /dev/null
+++ b/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm.c
@@ -0,0 +1,466 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdint.h>
+
+#include <lib/spinlock.h>
+
+#include <lib/mtk_init/mtk_init.h>
+#include <lib/pm/mtk_pm.h>
+#include "mt_cpu_pm.h"
+#include "mt_cpu_pm_cpc.h"
+#include "mt_cpu_pm_mbox.h"
+#include <mt_lp_rm.h>
+#include "mt_smp.h"
+#include <mtk_mmap_pool.h>
+#include <platform_def.h>
+
+/*
+ * The locker must use the bakery locker when cache turns off.
+ * Using spin_lock will gain better performance.
+ */
+#ifdef MT_CPU_PM_USING_BAKERY_LOCK
+DEFINE_BAKERY_LOCK(mt_cpu_pm_lock);
+#define plat_cpu_pm_lock_init()	bakery_lock_init(&mt_cpu_pm_lock)
+#define plat_cpu_pm_lock()	bakery_lock_get(&mt_cpu_pm_lock)
+#define plat_cpu_pm_unlock()	bakery_lock_release(&mt_cpu_pm_lock)
+#else
+spinlock_t mt_cpu_pm_lock;
+#define plat_cpu_pm_lock_init()
+#define plat_cpu_pm_lock()	spin_lock(&mt_cpu_pm_lock)
+#define plat_cpu_pm_unlock()	spin_unlock(&mt_cpu_pm_lock)
+#endif
+
+enum mt_pwr_node {
+	MT_PWR_NONMCUSYS = 0,
+	MT_PWR_MCUSYS_PDN,
+	MT_PWR_SUSPEND,
+	MT_PWR_SYSTEM_MEM,
+	MT_PWR_SYSTEM_PLL,
+	MT_PWR_SYSTEM_BUS,
+	MT_PWR_MAX,
+};
+
+#define CPU_PM_DEPD_INIT	BIT(0)
+#define CPU_PM_DEPD_READY	BIT(1)
+#define CPU_PM_PLAT_READY	BIT(2)
+
+#ifdef CPU_PM_TINYSYS_SUPPORT
+#define CPU_PM_INIT_READY	(CPU_PM_DEPD_INIT | CPU_PM_DEPD_READY)
+#define CPU_PM_LP_READY		(CPU_PM_INIT_READY | CPU_PM_PLAT_READY)
+#else
+#define CPU_PM_LP_READY		(CPU_PM_PLAT_READY)
+#endif
+
+#if CONFIG_MTK_PM_SUPPORT
+
+#if CONFIG_MTK_CPU_SUSPEND_EN || CONFIG_MTK_SMP_EN
+static void cpupm_cpu_resume_common(const struct mtk_cpupm_pwrstate *state)
+{
+	CPU_PM_ASSERT(state != NULL);
+	mtk_cpc_core_on_hint_clr(state->info.cpuid);
+}
+#endif
+
+#if CONFIG_MTK_SMP_EN
+static int cpupm_cpu_pwr_on_prepare(unsigned int cpu, uintptr_t entry)
+{
+	struct cpu_pwr_ctrl pwr_ctrl;
+
+	PER_CPU_PWR_CTRL(pwr_ctrl, cpu);
+	mt_smp_core_bootup_address_set(&pwr_ctrl, entry);
+	mt_smp_core_init_arch(0, cpu, 1, &pwr_ctrl);
+
+	return mt_smp_power_core_on(cpu, &pwr_ctrl);
+}
+
+static void cpupm_cpu_resume_smp(const struct mtk_cpupm_pwrstate *state)
+{
+	CPU_PM_ASSERT(state != NULL);
+
+	plat_cpu_pm_lock();
+	mmio_clrbits_32(CPC_MCUSYS_CPC_FLOW_CTRL_CFG,
+			GIC_WAKEUP_IGNORE(state->info.cpuid));
+	plat_cpu_pm_unlock();
+	cpupm_cpu_resume_common(state);
+}
+
+static void cpupm_cpu_suspend_smp(const struct mtk_cpupm_pwrstate *state)
+{
+	struct cpu_pwr_ctrl pwr_ctrl;
+
+	CPU_PM_ASSERT(state != NULL);
+
+	PER_CPU_PWR_CTRL(pwr_ctrl, state->info.cpuid);
+	mt_smp_power_core_off(&pwr_ctrl);
+	mmio_setbits_32(CPC_MCUSYS_CPC_FLOW_CTRL_CFG,
+			GIC_WAKEUP_IGNORE(state->info.cpuid));
+}
+
+static void cpupm_smp_init(unsigned int cpu, uintptr_t sec_entrypoint)
+{
+	unsigned int reg;
+	struct mtk_cpupm_pwrstate state = {
+		.info = {
+			.cpuid = cpu,
+			.mode = MTK_CPU_PM_SMP,
+		},
+		.pwr = {
+			.afflv = 0,
+			.state_id = 0,
+		},
+	};
+
+	reg = mmio_read_32(CPC_MCUSYS_CPC_FLOW_CTRL_CFG);
+	if ((reg & CPC_MCUSYS_CPC_RESET_PWR_ON_EN) != 0) {
+		INFO("[%s:%d][CPU_PM] reset pwr on is enabled then clear it!\n",
+		     __func__, __LINE__);
+		mmio_clrbits_32(CPC_MCUSYS_CPC_FLOW_CTRL_CFG, CPC_MCUSYS_CPC_RESET_PWR_ON_EN);
+	}
+
+	cpupm_cpu_pwr_on_prepare(cpu, sec_entrypoint);
+	cpupm_cpu_resume_smp(&state);
+}
+
+static struct mtk_cpu_smp_ops cpcv3_2_cpu_smp = {
+	.init = cpupm_smp_init,
+	.cpu_pwr_on_prepare = cpupm_cpu_pwr_on_prepare,
+	.cpu_on = cpupm_cpu_resume_smp,
+	.cpu_off = cpupm_cpu_suspend_smp,
+};
+
+#endif /* CONFIG_MTK_SMP_EN */
+
+#if CONFIG_MTK_CPU_SUSPEND_EN
+#define CPUPM_READY_MS		(40000)
+#define CPUPM_ARCH_TIME_MS(ms)	(ms * 1000 * SYS_COUNTER_FREQ_IN_MHZ)
+#define CPUPM_BOOTUP_TIME_THR	CPUPM_ARCH_TIME_MS(CPUPM_READY_MS)
+
+static int mt_pwr_nodes[MT_PWR_MAX];
+static int plat_mt_lp_cpu_rc;
+static unsigned int cpu_pm_status;
+static unsigned int plat_prev_stateid;
+
+static int mcusys_prepare_suspend(const struct mtk_cpupm_pwrstate *state)
+{
+	unsigned int stateid = state->pwr.state_id;
+
+	if (mtk_cpc_mcusys_off_prepare() != CPC_SUCCESS) {
+		goto mt_pwr_mcusysoff_break;
+	}
+
+	if (!IS_PLAT_SUSPEND_ID(stateid)) {
+		if (mt_pwr_nodes[MT_PWR_SYSTEM_MEM] != 0) {
+			stateid = MT_PLAT_PWR_STATE_SYSTEM_MEM;
+		} else if (mt_pwr_nodes[MT_PWR_SYSTEM_PLL] != 0) {
+			stateid = MT_PLAT_PWR_STATE_SYSTEM_PLL;
+		} else if (mt_pwr_nodes[MT_PWR_SYSTEM_BUS] != 0) {
+			stateid = MT_PLAT_PWR_STATE_SYSTEM_BUS;
+		} else if (mt_pwr_nodes[MT_PWR_SUSPEND] != 0) {
+			stateid = MT_PLAT_PWR_STATE_SUSPEND;
+		} else {
+			stateid = MT_PLAT_PWR_STATE_MCUSYS;
+		}
+	}
+
+	plat_prev_stateid = stateid;
+	plat_mt_lp_cpu_rc = mt_lp_rm_find_and_run_constraint(0, state->info.cpuid, stateid, NULL);
+
+	if (plat_mt_lp_cpu_rc < 0) {
+		goto mt_pwr_mcusysoff_reflect;
+	}
+
+#ifdef CPU_PM_TINYSYS_SUPPORT
+	mtk_set_cpu_pm_preffered_cpu(state->info.cpuid);
+#endif
+	return MTK_CPUPM_E_OK;
+
+mt_pwr_mcusysoff_reflect:
+	mtk_cpc_mcusys_off_reflect();
+mt_pwr_mcusysoff_break:
+	plat_mt_lp_cpu_rc = -1;
+
+	return MTK_CPUPM_E_FAIL;
+}
+
+static int mcusys_prepare_resume(const struct mtk_cpupm_pwrstate *state)
+{
+	if (plat_mt_lp_cpu_rc < 0) {
+		return MTK_CPUPM_E_FAIL;
+	}
+
+	mt_lp_rm_reset_constraint(plat_mt_lp_cpu_rc, state->info.cpuid, plat_prev_stateid);
+	mtk_cpc_mcusys_off_reflect();
+	return MTK_CPUPM_E_OK;
+}
+
+static unsigned int cpupm_do_pstate_off(const mtk_pstate_type psci_state,
+					const struct mtk_cpupm_pwrstate *state)
+{
+	unsigned int pstate = MT_CPUPM_PWR_DOMAIN_CORE;
+
+	if (!state || (state->pwr.afflv > PLAT_MAX_PWR_LVL)) {
+		CPU_PM_ASSERT(0);
+	}
+
+	switch (state->pwr.state_id) {
+	case MT_PLAT_PWR_STATE_SYSTEM_MEM:
+		mt_pwr_nodes[MT_PWR_SYSTEM_MEM] += 1;
+		break;
+	case MT_PLAT_PWR_STATE_SYSTEM_PLL:
+		mt_pwr_nodes[MT_PWR_SYSTEM_PLL] += 1;
+		break;
+	case MT_PLAT_PWR_STATE_SYSTEM_BUS:
+		mt_pwr_nodes[MT_PWR_SYSTEM_BUS] += 1;
+		break;
+	case MT_PLAT_PWR_STATE_SUSPEND:
+		mt_pwr_nodes[MT_PWR_SUSPEND] += 1;
+		break;
+	default:
+		if (!IS_MT_PLAT_PWR_STATE_MCUSYS(state->pwr.state_id) &&
+		    !IS_PLAT_SYSTEM_SUSPEND(state->pwr.afflv)) {
+			plat_cpu_pm_lock();
+			mt_pwr_nodes[MT_PWR_NONMCUSYS] += 1;
+			flush_dcache_range((uintptr_t)&mt_pwr_nodes[MT_PWR_NONMCUSYS],
+					   sizeof(mt_pwr_nodes[MT_PWR_NONMCUSYS]));
+			plat_cpu_pm_unlock();
+		}
+		break;
+	}
+
+	if ((mt_pwr_nodes[MT_PWR_NONMCUSYS] == 0) && IS_PLAT_MCUSYSOFF_AFFLV(state->pwr.afflv)) {
+		/* Prepare to power down mcusys */
+		if (mcusys_prepare_suspend(state) == MTK_CPUPM_E_OK) {
+			mt_pwr_nodes[MT_PWR_MCUSYS_PDN] += 1;
+			flush_dcache_range((uintptr_t)&mt_pwr_nodes[MT_PWR_MCUSYS_PDN],
+					   sizeof(mt_pwr_nodes[MT_PWR_MCUSYS_PDN]));
+			pstate |= (MT_CPUPM_PWR_DOMAIN_MCUSYS | MT_CPUPM_PWR_DOMAIN_CLUSTER);
+		}
+	}
+
+	if (state->pwr.afflv >= PLAT_MT_CPU_SUSPEND_CLUSTER) {
+		pstate |= MT_CPUPM_PWR_DOMAIN_CLUSTER;
+	}
+
+	if (psci_get_pstate_pwrlvl(psci_state) >= PLAT_MT_CPU_SUSPEND_CLUSTER) {
+		pstate |= MT_CPUPM_PWR_DOMAIN_PERCORE_DSU;
+	}
+
+	return pstate;
+}
+
+static unsigned int cpupm_do_pstate_on(const mtk_pstate_type psci_state,
+				       const struct mtk_cpupm_pwrstate *state)
+{
+	unsigned int pstate = MT_CPUPM_PWR_DOMAIN_CORE;
+
+	CPU_PM_ASSERT(state != NULL);
+
+	if (state->pwr.afflv > PLAT_MAX_PWR_LVL) {
+		CPU_PM_ASSERT(0);
+	}
+
+	if (mt_pwr_nodes[MT_PWR_MCUSYS_PDN] != 0) {
+		mt_pwr_nodes[MT_PWR_MCUSYS_PDN] = 0;
+		flush_dcache_range((uintptr_t)&mt_pwr_nodes[MT_PWR_MCUSYS_PDN],
+				   sizeof(mt_pwr_nodes[MT_PWR_MCUSYS_PDN]));
+		pstate |= (MT_CPUPM_PWR_DOMAIN_MCUSYS | MT_CPUPM_PWR_DOMAIN_CLUSTER);
+		mcusys_prepare_resume(state);
+	}
+
+	if (state->pwr.afflv >= PLAT_MT_CPU_SUSPEND_CLUSTER) {
+		pstate |= MT_CPUPM_PWR_DOMAIN_CLUSTER;
+	}
+
+	switch (state->pwr.state_id) {
+	case MT_PLAT_PWR_STATE_SYSTEM_MEM:
+		mt_pwr_nodes[MT_PWR_SYSTEM_MEM] -= 1;
+		CPU_PM_ASSERT(mt_pwr_nodes[MT_PWR_SYSTEM_MEM] >= 0);
+		break;
+	case MT_PLAT_PWR_STATE_SYSTEM_PLL:
+		mt_pwr_nodes[MT_PWR_SYSTEM_PLL] -= 1;
+		CPU_PM_ASSERT(mt_pwr_nodes[MT_PWR_SYSTEM_PLL] >= 0);
+		break;
+	case MT_PLAT_PWR_STATE_SYSTEM_BUS:
+		mt_pwr_nodes[MT_PWR_SYSTEM_BUS] -= 1;
+		CPU_PM_ASSERT(mt_pwr_nodes[MT_PWR_SYSTEM_BUS] >= 0);
+		break;
+	case MT_PLAT_PWR_STATE_SUSPEND:
+		mt_pwr_nodes[MT_PWR_SUSPEND] -= 1;
+		CPU_PM_ASSERT(mt_pwr_nodes[MT_PWR_SUSPEND] >= 0);
+		break;
+	default:
+		if (!IS_MT_PLAT_PWR_STATE_MCUSYS(state->pwr.state_id) &&
+		    !IS_PLAT_SYSTEM_SUSPEND(state->pwr.afflv)) {
+			plat_cpu_pm_lock();
+			mt_pwr_nodes[MT_PWR_NONMCUSYS] -= 1;
+			flush_dcache_range((uintptr_t)&mt_pwr_nodes[MT_PWR_NONMCUSYS],
+					   sizeof(mt_pwr_nodes[MT_PWR_NONMCUSYS]));
+			plat_cpu_pm_unlock();
+		}
+		break;
+	}
+
+	if (IS_PLAT_SYSTEM_SUSPEND(state->pwr.afflv) ||
+	    (IS_PLAT_SYSTEM_RETENTION(state->pwr.afflv) && (mt_pwr_nodes[MT_PWR_SUSPEND] > 0))) {
+		mtk_cpc_time_sync();
+	}
+
+	if (mt_pwr_nodes[MT_PWR_NONMCUSYS] < 0) {
+		CPU_PM_ASSERT(0);
+	}
+
+	pstate |= MT_CPUPM_PWR_DOMAIN_PERCORE_DSU;
+
+	return pstate;
+}
+
+static void cpupm_cpu_resume(const struct mtk_cpupm_pwrstate *state)
+{
+	cpupm_cpu_resume_common(state);
+}
+
+static void cpupm_mcusys_resume(const struct mtk_cpupm_pwrstate *state)
+{
+	assert(state != NULL);
+}
+
+static void cpupm_mcusys_suspend(const struct mtk_cpupm_pwrstate *state)
+{
+	assert(state != NULL);
+}
+
+static unsigned int cpupm_get_pstate(enum mt_cpupm_pwr_domain domain,
+				     const mtk_pstate_type psci_state,
+				     const struct mtk_cpupm_pwrstate *state)
+{
+	unsigned int pstate = 0;
+
+	if (state == NULL) {
+		return 0;
+	}
+
+	if (state->info.mode == MTK_CPU_PM_SMP) {
+		pstate = MT_CPUPM_PWR_DOMAIN_CORE;
+	} else {
+		if (domain == CPUPM_PWR_OFF) {
+			pstate = cpupm_do_pstate_off(psci_state, state);
+		} else if (domain == CPUPM_PWR_ON) {
+			pstate = cpupm_do_pstate_on(psci_state, state);
+		} else {
+			INFO("[%s:%d][CPU_PM] unknown pwr domain :%d\n",
+			     __func__, __LINE__, domain);
+			assert(0);
+		}
+	}
+	return pstate;
+}
+
+static int cpupm_init(void)
+{
+	int ret = MTK_CPUPM_E_OK;
+
+#ifdef CPU_PM_TINYSYS_SUPPORT
+	int status;
+
+	if ((cpu_pm_status & CPU_PM_INIT_READY) == CPU_PM_INIT_READY) {
+		return MTK_CPUPM_E_OK;
+	}
+
+	if (!(cpu_pm_status & CPU_PM_DEPD_INIT)) {
+		status = mtk_lp_depd_condition(CPUPM_MBOX_WAIT_DEV_INIT);
+		if (status == 0) {
+			plat_cpu_pm_lock();
+			cpu_pm_status |= CPU_PM_DEPD_INIT;
+			plat_cpu_pm_unlock();
+		}
+	}
+
+	if ((cpu_pm_status & CPU_PM_DEPD_INIT) && !(cpu_pm_status & CPU_PM_DEPD_READY)) {
+		status = mtk_lp_depd_condition(CPUPM_MBOX_WAIT_TASK_READY);
+		if (status == 0) {
+			plat_cpu_pm_lock();
+			cpu_pm_status |= CPU_PM_DEPD_READY;
+			plat_cpu_pm_unlock();
+		}
+	}
+
+	ret = ((cpu_pm_status & CPU_PM_INIT_READY) == CPU_PM_INIT_READY) ?
+	      MTK_CPUPM_E_OK : MTK_CPUPM_E_FAIL;
+#endif
+	return ret;
+}
+
+static int cpupm_pwr_state_valid(unsigned int afflv, unsigned int state)
+{
+	if (cpu_pm_status == CPU_PM_LP_READY) {
+		return MTK_CPUPM_E_OK;
+	}
+
+	if (cpupm_init() != MTK_CPUPM_E_OK) {
+		return MTK_CPUPM_E_FAIL;
+	}
+
+	if (read_cntpct_el0() >= (uint64_t)CPUPM_BOOTUP_TIME_THR) {
+		plat_cpu_pm_lock();
+		cpu_pm_status |= CPU_PM_PLAT_READY;
+		plat_cpu_pm_unlock();
+	}
+
+	if (!IS_PLAT_SYSTEM_SUSPEND(afflv) && (cpu_pm_status & CPU_PM_PLAT_READY) == 0) {
+		return MTK_CPUPM_E_FAIL;
+	}
+
+	return MTK_CPUPM_E_OK;
+}
+
+static struct mtk_cpu_pm_ops cpcv3_2_mcdi = {
+	.get_pstate = cpupm_get_pstate,
+	.pwr_state_valid = cpupm_pwr_state_valid,
+	.cpu_resume = cpupm_cpu_resume,
+	.mcusys_suspend = cpupm_mcusys_suspend,
+	.mcusys_resume = cpupm_mcusys_resume,
+};
+#endif /* CONFIG_MTK_CPU_SUSPEND_EN */
+
+#endif /* CONFIG_MTK_PM_SUPPORT */
+
+/*
+ * Depend on mtk pm methodology, the psci op init must
+ * be invoked after cpu pm to avoid initialization fail.
+ */
+int mt_plat_cpu_pm_init(void)
+{
+	plat_cpu_pm_lock_init();
+
+	mtk_cpc_init();
+#if CONFIG_MTK_PM_SUPPORT
+
+#if CONFIG_MTK_CPU_SUSPEND_EN
+	register_cpu_pm_ops(CPU_PM_FN, &cpcv3_2_mcdi);
+#endif /* CONFIG_MTK_CPU_SUSPEND_EN */
+
+#if CONFIG_MTK_SMP_EN
+	register_cpu_smp_ops(CPU_PM_FN, &cpcv3_2_cpu_smp);
+#endif /* CONFIG_MTK_SMP_EN */
+
+#endif /* CONFIG_MTK_PM_SUPPORT */
+
+	INFO("[%s:%d] - CPU PM INIT finished\n", __func__, __LINE__);
+	return 0;
+}
+MTK_ARCH_INIT(mt_plat_cpu_pm_init);
+
+static const mmap_region_t cpu_pm_mmap[] MTK_MMAP_SECTION = {
+#ifdef CPU_PM_TINYSYS_SUPPORT
+#if CONFIG_MTK_PM_SUPPORT && CONFIG_MTK_CPU_SUSPEND_EN
+	MAP_REGION_FLAT(CPU_EB_TCM_BASE, CPU_EB_TCM_SIZE, MT_DEVICE | MT_RW | MT_SECURE),
+#endif
+#endif
+	{0}
+};
+DECLARE_MTK_MMAP_REGIONS(cpu_pm_mmap);
diff --git a/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm.h b/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm.h
new file mode 100644
index 0000000..4d99df1
--- /dev/null
+++ b/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm.h
@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MT_CPU_PM_H
+#define MT_CPU_PM_H
+
+#include <assert.h>
+#include <mcucfg.h>
+#include <platform_def.h>
+
+/*
+ * After ARM v8.2, the cache will turn off automatically when powering down CPU. Therefore, there
+ * is no doubt to use the spin_lock here.
+ */
+#if !HW_ASSISTED_COHERENCY
+#define MT_CPU_PM_USING_BAKERY_LOCK
+#endif
+
+#define CPU_PM_FN (MTK_CPUPM_FN_CPUPM_GET_PWR_STATE | \
+		   MTK_CPUPM_FN_PWR_STATE_VALID | \
+		   MTK_CPUPM_FN_PWR_ON_CORE_PREPARE | \
+		   MTK_CPUPM_FN_RESUME_CORE | \
+		   MTK_CPUPM_FN_SUSPEND_MCUSYS | \
+		   MTK_CPUPM_FN_RESUME_MCUSYS | \
+		   MTK_CPUPM_FN_SMP_INIT | \
+		   MTK_CPUPM_FN_SMP_CORE_ON | \
+		   MTK_CPUPM_FN_SMP_CORE_OFF)
+
+#define CPU_PM_ASSERT(_cond) ({ \
+	if (!(_cond)) { \
+		INFO("[%s:%d] - %s\n", __func__, __LINE__, #_cond); \
+		panic(); \
+	} })
+
+#define CPC_PWR_MASK_MCUSYS_MP0		(0xC001)
+
+#define PER_CPU_PWR_DATA(ctrl, cluster, core) \
+	do { \
+		ctrl.rvbaraddr_l = CORE_RVBRADDR_##cluster##_##core##_L; \
+		ctrl.arch_addr = MCUCFG_MP0_CLUSTER_CFG5; \
+		ctrl.pwpr = SPM_MP##cluster##_CPU##core##_PWR_CON; \
+	} while (0)
+
+#define PER_CPU_PWR_CTRL(ctrl, cpu) ({ \
+	switch (cpu) { \
+	case 0: \
+		PER_CPU_PWR_DATA(ctrl, 0, 0); \
+		break; \
+	case 1: \
+		PER_CPU_PWR_DATA(ctrl, 0, 1); \
+		break; \
+	case 2: \
+		PER_CPU_PWR_DATA(ctrl, 0, 2); \
+		break; \
+	case 3: \
+		PER_CPU_PWR_DATA(ctrl, 0, 3); \
+		break; \
+	case 4: \
+		PER_CPU_PWR_DATA(ctrl, 0, 4); \
+		break; \
+	case 5: \
+		PER_CPU_PWR_DATA(ctrl, 0, 5); \
+		break; \
+	case 6: \
+		PER_CPU_PWR_DATA(ctrl, 0, 6); \
+		break; \
+	case 7: \
+		PER_CPU_PWR_DATA(ctrl, 0, 7); \
+		break; \
+	default: \
+		assert(0); \
+		break; \
+	} })
+
+
+/* MCUSYS DREQ BIG VPROC ISO control */
+#define DREQ20_BIG_VPROC_ISO		(MCUCFG_BASE + 0xad8c)
+
+/* Definition about bootup address for each core CORE_RVBRADDR_clusterid_cpuid */
+#define CORE_RVBRADDR_0_0_L		(MCUCFG_BASE + 0xc900)
+#define CORE_RVBRADDR_0_1_L		(MCUCFG_BASE + 0xc908)
+#define CORE_RVBRADDR_0_2_L		(MCUCFG_BASE + 0xc910)
+#define CORE_RVBRADDR_0_3_L		(MCUCFG_BASE + 0xc918)
+#define CORE_RVBRADDR_0_4_L		(MCUCFG_BASE + 0xc920)
+#define CORE_RVBRADDR_0_5_L		(MCUCFG_BASE + 0xc928)
+#define CORE_RVBRADDR_0_6_L		(MCUCFG_BASE + 0xc930)
+#define CORE_RVBRADDR_0_7_L		(MCUCFG_BASE + 0xc938)
+#define MCUCFG_MP0_CLUSTER_CFG5		(MCUCFG_BASE + 0xc8e4)
+
+struct cpu_pwr_ctrl {
+	unsigned int rvbaraddr_l;
+	unsigned int arch_addr;
+	unsigned int pwpr;
+};
+
+#define MCUSYS_STATUS_PDN		BIT(0)
+#define MCUSYS_STATUS_CPUSYS_PROTECT	BIT(8)
+#define MCUSYS_STATUS_MCUSYS_PROTECT	BIT(9)
+
+/* cpu_pm function ID */
+enum mt_cpu_pm_user_id {
+	MCUSYS_STATUS,
+	CPC_COMMAND,
+};
+
+/* 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 /* MT_CPU_PM_H */
diff --git a/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm_cpc.c b/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm_cpc.c
new file mode 100644
index 0000000..4cc2203
--- /dev/null
+++ b/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm_cpc.c
@@ -0,0 +1,253 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+
+#include <drivers/delay_timer.h>
+
+#include "mt_cpu_pm.h"
+#include "mt_cpu_pm_cpc.h"
+#include "mt_smp.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(int prot_req, int resp_reg, int resp_ofs)
+{
+	unsigned int staus;
+	unsigned int retry = 0;
+
+	while (retry < RETRY_CNT_MAX) {
+		retry++;
+
+		mmio_write_32(CPC_MCUSYS_LAST_CORE_REQ, prot_req);
+
+		udelay(1);
+
+		staus = (mmio_read_32(resp_reg) >> resp_ofs) & CPC_PROT_RESP_MASK;
+
+		if (staus == PROT_SUCCESS) {
+			return CPC_SUCCESS;
+		} else if (staus == PROT_GIVEUP) {
+			return CPC_ERR_FAIL;
+		}
+	}
+
+	return CPC_ERR_TIMEOUT;
+}
+
+static 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);
+}
+
+static 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(void)
+{
+	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(void)
+{
+	mmio_write_32(CPC_MCUSYS_PWR_ON_MASK, CPUSYS_PROT_CLR);
+}
+
+static void mtk_cpc_cluster_cnt_backup(void)
+{
+	/* single cluster */
+	uint32_t backup_cnt = mmio_read_32(CPC_CLUSTER_CNT_BACKUP);
+	uint32_t curr_cnt = mmio_read_32(CPC_MCUSYS_CLUSTER_COUNTER);
+
+	if ((curr_cnt & 0x7fff) == 0) {
+		curr_cnt = (curr_cnt >> 16) & 0x7fff;
+	} else {
+		curr_cnt = curr_cnt & 0x7fff;
+	}
+
+	mmio_write_32(CPC_CLUSTER_CNT_BACKUP, backup_cnt + curr_cnt);
+	mmio_write_32(CPC_MCUSYS_CLUSTER_COUNTER_CLR, 0x3);
+}
+
+static inline void mtk_cpc_mcusys_off_enable(bool enable)
+{
+	mmio_write_32(CPC_MCUSYS_PWR_CTRL, enable ? 1 : 0);
+}
+
+void mtk_cpc_mcusys_off_reflect(void)
+{
+	mtk_cpc_mcusys_off_enable(false);
+	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_enable(true);
+
+	return CPC_SUCCESS;
+}
+
+void mtk_cpc_core_on_hint_set(int cpu)
+{
+	mmio_write_32(CPC_MCUSYS_CPU_ON_SW_HINT_SET, BIT(cpu));
+}
+
+void mtk_cpc_core_on_hint_clr(int cpu)
+{
+	mmio_write_32(CPC_MCUSYS_CPU_ON_SW_HINT_CLR, BIT(cpu));
+}
+
+static void mtk_cpc_dump_timestamp(void)
+{
+	unsigned int id;
+
+	for (id = 0; 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(unsigned int cfg, unsigned int data)
+{
+	switch (cfg) {
+	case CPC_SMC_CONFIG_PROF:
+		if (data) {
+			mmio_setbits_32(CPC_MCUSYS_CPC_DBG_SETTING, CPC_PROF_EN);
+		} else {
+			mmio_clrbits_32(CPC_MCUSYS_CPC_DBG_SETTING, CPC_PROF_EN);
+		}
+		break;
+	case CPC_SMC_CONFIG_AUTO_OFF:
+		if (data) {
+			mmio_setbits_32(CPC_MCUSYS_CPC_FLOW_CTRL_CFG, CPC_AUTO_OFF_EN);
+			cpc.auto_off = 1;
+		} else {
+			mmio_setbits_32(CPC_MCUSYS_CPC_FLOW_CTRL_CFG, CPC_AUTO_OFF_EN);
+			cpc.auto_off = 0;
+		}
+		break;
+	case CPC_SMC_CONFIG_AUTO_OFF_THRES:
+		cpc.auto_thres_tick = US_TO_TICKS(data);
+		mmio_write_32(CPC_MCUSYS_CPC_OFF_THRES, cpc.auto_thres_tick);
+		break;
+	case CPC_SMC_CONFIG_CNT_CLR:
+		mmio_write_32(CPC_MCUSYS_CLUSTER_COUNTER_CLR, 0x3);
+		break;
+	case CPC_SMC_CONFIG_TIME_SYNC:
+		mtk_cpc_time_sync();
+		break;
+	default:
+		break;
+	}
+}
+
+static unsigned int mtk_cpc_read_config(unsigned int cfg)
+{
+	unsigned int res = 0;
+
+	switch (cfg) {
+	case CPC_SMC_CONFIG_PROF:
+		res = (mmio_read_32(CPC_MCUSYS_CPC_DBG_SETTING) & CPC_PROF_EN) ? 1 : 0;
+		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:
+	default:
+		break;
+	}
+
+	return res;
+}
+
+uint64_t mtk_cpc_handler(uint64_t act, uint64_t arg1, uint64_t arg2)
+{
+	uint64_t res = 0;
+
+	switch (act) {
+	case CPC_SMC_EVENT_CPC_CONFIG:
+		mtk_cpc_config((unsigned int)arg1, (unsigned int)arg2);
+		break;
+	case CPC_SMC_EVENT_READ_CONFIG:
+		res = mtk_cpc_read_config((unsigned int)arg1);
+		break;
+	case CPC_SMC_EVENT_GIC_DPG_SET:
+		/* isolated_status = x2; */
+	default:
+		break;
+	}
+
+	return res;
+}
+
+uint64_t mtk_cpc_trace_dump(uint64_t act, uint64_t arg1, uint64_t arg2)
+{
+	switch (act) {
+	case CPC_SMC_EVENT_DUMP_TRACE_DATA:
+		mtk_cpc_dump_timestamp();
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+void mtk_cpc_init(void)
+{
+#if CONFIG_MTK_SMP_EN
+	mt_smp_init();
+#endif
+	mmio_setbits_32(CPC_MCUSYS_CPC_DBG_SETTING, (CPC_DBG_EN | CPC_CALC_EN));
+
+	cpc.auto_off = 1;
+	mmio_setbits_32(CPC_MCUSYS_CPC_FLOW_CTRL_CFG, (CPC_OFF_PRE_EN |
+						      ((cpc.auto_off > 0) ? CPC_AUTO_OFF_EN : 0)));
+
+	mtk_cpc_config(CPC_SMC_CONFIG_AUTO_OFF_THRES, 8000);
+
+	/* enable CPC */
+	mmio_setbits_32(CPC_MCUSYS_CPC_FLOW_CTRL_CFG, CPC_CTRL_ENABLE);
+	mmio_setbits_32(CPC_MCUSYS_CPC_FLOW_CTRL_CFG, SSPM_CORE_PWR_ON_EN);
+}
diff --git a/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm_cpc.h b/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm_cpc.h
new file mode 100644
index 0000000..3004f41
--- /dev/null
+++ b/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm_cpc.h
@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) 2022, 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 <mcucfg.h>
+#include <platform_def.h>
+
+#define NEED_CPUSYS_PROT_WORKAROUND	(1)
+
+/* system sram registers */
+#define CPUIDLE_SRAM_REG(r)	(CPU_IDLE_SRAM_BASE + (r))
+
+/* db dump */
+#define CPC_TRACE_SIZE		(0x20)
+#define CPC_TRACE_ID_NUM	(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	(0x3)
+#define CPUSYS_RESP_OFS		(16)
+#define MCUSYS_RESP_OFS		(30)
+
+#define RETRY_CNT_MAX		(1000)
+
+#define PROT_RETRY		(0)
+#define PROT_SUCCESS		(1)
+#define PROT_GIVEUP		(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 mcusys_cpc_lastcore_prot_status {
+	CPC_SUCCESS = 0,
+	CPC_ERR_FAIL,
+	CPC_ERR_TIMEOUT,
+	NF_CPC_ERR,
+};
+
+enum mcusys_cpc_smc_events {
+	CPC_SMC_EVENT_DUMP_TRACE_DATA,
+	CPC_SMC_EVENT_GIC_DPG_SET,
+	CPC_SMC_EVENT_CPC_CONFIG,
+	CPC_SMC_EVENT_READ_CONFIG,
+	NF_CPC_SMC_EVENT,
+};
+
+enum mcusys_cpc_smc_config {
+	CPC_SMC_CONFIG_PROF,
+	CPC_SMC_CONFIG_AUTO_OFF,
+	CPC_SMC_CONFIG_AUTO_OFF_THRES,
+	CPC_SMC_CONFIG_CNT_CLR,
+	CPC_SMC_CONFIG_TIME_SYNC,
+	NF_CPC_SMC_CONFIG,
+};
+
+#define US_TO_TICKS(us)		((us) * 13)
+#define TICKS_TO_US(tick)	((tick) / 13)
+
+int mtk_cpu_pm_cluster_prot_aquire(void);
+void mtk_cpu_pm_cluster_prot_release(void);
+
+void mtk_cpc_mcusys_off_reflect(void);
+int mtk_cpc_mcusys_off_prepare(void);
+
+void mtk_cpc_core_on_hint_set(int cpu);
+void mtk_cpc_core_on_hint_clr(int cpu);
+void mtk_cpc_time_sync(void);
+
+uint64_t mtk_cpc_handler(uint64_t act, uint64_t arg1, uint64_t arg2);
+uint64_t mtk_cpc_trace_dump(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/drivers/cpu_pm/cpcv3_2/mt_cpu_pm_mbox.c b/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm_mbox.c
new file mode 100644
index 0000000..4d67e7b
--- /dev/null
+++ b/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm_mbox.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+
+#include <lib/mmio.h>
+
+#include "mt_cpu_pm_mbox.h"
+#include <platform_def.h>
+
+#ifdef __GNUC__
+#define MCDI_LIKELY(x)		__builtin_expect(!!(x), 1)
+#define MCDI_UNLIKELY(x)	__builtin_expect(!!(x), 0)
+#else
+#define MCDI_LIKELY(x)		(x)
+#define MCDI_UNLIKELY(x)	(x)
+#endif
+
+#define MCUPM_MBOX_3_BASE		(CPU_EB_TCM_BASE + CPU_EB_MBOX3_OFFSET)
+#define MCUPM_MBOX_WRITE(id, val)	mmio_write_32(MCUPM_MBOX_3_BASE + 4 * (id), val)
+#define MCUPM_MBOX_READ(id)		mmio_read_32(MCUPM_MBOX_3_BASE + 4 * (id))
+
+void mtk_set_mcupm_pll_mode(unsigned int mode)
+{
+	if (mode < NF_MCUPM_ARMPLL_MODE) {
+		MCUPM_MBOX_WRITE(MCUPM_MBOX_ARMPLL_MODE, mode);
+	}
+}
+
+int mtk_get_mcupm_pll_mode(void)
+{
+	return MCUPM_MBOX_READ(MCUPM_MBOX_ARMPLL_MODE);
+}
+
+void mtk_set_mcupm_buck_mode(unsigned int mode)
+{
+	if (mode < NF_MCUPM_BUCK_MODE) {
+		MCUPM_MBOX_WRITE(MCUPM_MBOX_BUCK_MODE, mode);
+	}
+}
+
+int mtk_get_mcupm_buck_mode(void)
+{
+	return MCUPM_MBOX_READ(MCUPM_MBOX_BUCK_MODE);
+}
+
+void mtk_set_cpu_pm_preffered_cpu(unsigned int cpuid)
+{
+	return MCUPM_MBOX_WRITE(MCUPM_MBOX_WAKEUP_CPU, cpuid);
+}
+
+unsigned int mtk_get_cpu_pm_preffered_cpu(void)
+{
+	return MCUPM_MBOX_READ(MCUPM_MBOX_WAKEUP_CPU);
+}
+
+static int mtk_wait_mbox_init_done(void)
+{
+	int status = MCUPM_MBOX_READ(MCUPM_MBOX_TASK_STA);
+
+	if (status != MCUPM_TASK_INIT) {
+		return status;
+	}
+
+	mtk_set_mcupm_pll_mode(MCUPM_ARMPLL_OFF);
+	mtk_set_mcupm_buck_mode(MCUPM_BUCK_OFF_MODE);
+
+	MCUPM_MBOX_WRITE(MCUPM_MBOX_PWR_CTRL_EN, (MCUPM_MCUSYS_CTRL | MCUPM_CM_CTRL |
+						 MCUPM_BUCK_CTRL | MCUPM_ARMPLL_CTRL));
+
+	return status;
+}
+
+int mtk_lp_depd_condition(enum cpupm_mbox_depd_type type)
+{
+	int status;
+
+	if (type == CPUPM_MBOX_WAIT_DEV_INIT) {
+		status = mtk_wait_mbox_init_done();
+		if (MCDI_UNLIKELY(status != MCUPM_TASK_INIT)) {
+			return -ENXIO;
+		}
+		MCUPM_MBOX_WRITE(MCUPM_MBOX_AP_READY, 1);
+	} else if (type == CPUPM_MBOX_WAIT_TASK_READY) {
+		status = MCUPM_MBOX_READ(MCUPM_MBOX_TASK_STA);
+		if (MCDI_UNLIKELY((status != MCUPM_TASK_WAIT) &&
+				  (status != MCUPM_TASK_INIT_FINISH))) {
+			return -ENXIO;
+		}
+	}
+	return 0;
+}
diff --git a/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm_mbox.h b/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm_mbox.h
new file mode 100644
index 0000000..72be6bd
--- /dev/null
+++ b/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_cpu_pm_mbox.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MT_CPU_PM_MBOX_H
+#define MT_CPU_PM_MBOX_H
+
+#include <lib/utils_def.h>
+
+/* MCUPM Mbox */
+/* AP Write */
+#define MCUPM_MBOX_AP_READY		(0)
+#define MCUPM_MBOX_RESERVED_1		(1)
+#define MCUPM_MBOX_RESERVED_2		(2)
+#define MCUPM_MBOX_RESERVED_3		(3)
+#define MCUPM_MBOX_PWR_CTRL_EN		(4)
+#define MCUPM_MBOX_L3_CACHE_MODE	(5)
+#define MCUPM_MBOX_BUCK_MODE		(6)
+#define MCUPM_MBOX_ARMPLL_MODE		(7)
+/* AP Read */
+#define MCUPM_MBOX_TASK_STA		(8)
+#define MCUPM_MBOX_RESERVED_9		(9)
+#define MCUPM_MBOX_RESERVED_10		(10)
+#define MCUPM_MBOX_RESERVED_11		(11)
+#define MCUPM_MBOX_WAKEUP_CPU		(12)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_PWR_CTRL_EN (4) */
+#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		(BIT(3) - 1)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_L3_CACHE_MODE (5) */
+#define MCUPM_L3_OFF_MODE		(0) /* default */
+#define MCUPM_L3_DORMANT_MODE		(1)
+#define NF_MCUPM_L3_MODE		(2)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_BUCK_MODE (6) */
+#define MCUPM_BUCK_NORMAL_MODE		(0) /* default */
+#define MCUPM_BUCK_LP_MODE		(1)
+#define MCUPM_BUCK_OFF_MODE		(2)
+#define NF_MCUPM_BUCK_MODE		(3)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_ARMPLL_MODE (7) */
+#define MCUPM_ARMPLL_ON			(0) /* default */
+#define MCUPM_ARMPLL_GATING		(1)
+#define MCUPM_ARMPLL_OFF		(2)
+#define NF_MCUPM_ARMPLL_MODE		(3)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_TASK_STA (9) */
+#define MCUPM_TASK_UNINIT		(0)
+#define MCUPM_TASK_INIT			(1)
+#define MCUPM_TASK_INIT_FINISH		(2)
+#define MCUPM_TASK_WAIT			(3)
+#define MCUPM_TASK_RUN			(4)
+#define MCUPM_TASK_PAUSE		(5)
+
+
+void mtk_set_mcupm_pll_mode(unsigned int mode);
+int mtk_get_mcupm_pll_mode(void);
+
+void mtk_set_mcupm_buck_mode(unsigned int mode);
+int mtk_get_mcupm_buck_mode(void);
+
+void mtk_set_cpu_pm_preffered_cpu(unsigned int cpuid);
+unsigned int mtk_get_cpu_pm_preffered_cpu(void);
+
+enum cpupm_mbox_depd_type {
+	CPUPM_MBOX_WAIT_DEV_INIT,
+	CPUPM_MBOX_WAIT_TASK_READY,
+};
+
+int mtk_lp_depd_condition(enum cpupm_mbox_depd_type type);
+
+#endif /* MT_CPU_PM_MBOX_H */
diff --git a/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_smp.c b/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_smp.c
new file mode 100644
index 0000000..a1d9c31
--- /dev/null
+++ b/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_smp.c
@@ -0,0 +1,93 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <plat/common/platform.h>
+
+#include <lib/pm/mtk_pm.h>
+#include <mcucfg.h>
+#include "mt_cpu_pm.h"
+#include "mt_smp.h"
+
+static inline int is_core_power_status_on(unsigned int cpuid)
+{
+	return !!(mmio_read_32(CPU_PWR_STATUS) & BIT(cpuid));
+}
+
+void mt_smp_core_init_arch(unsigned int cluster, unsigned int cpu, int arm64,
+			   struct cpu_pwr_ctrl *pwr_ctrl)
+{
+	CPU_PM_ASSERT(cluster == 0);
+	CPU_PM_ASSERT(pwr_ctrl != NULL);
+
+	/* aa64naa32 in bits[16:23] */
+	if (arm64 != 0) {
+		mmio_setbits_32(pwr_ctrl->arch_addr, 1 << (16 + cpu));
+	} else {
+		mmio_clrbits_32(pwr_ctrl->arch_addr, 1 << (16 + cpu));
+	}
+}
+
+void mt_smp_core_bootup_address_set(struct cpu_pwr_ctrl *pwr_ctrl, uintptr_t entry)
+{
+	CPU_PM_ASSERT(pwr_ctrl != NULL);
+
+	/* Set bootup address */
+	mmio_write_32(pwr_ctrl->rvbaraddr_l, entry);
+}
+
+int mt_smp_power_core_on(unsigned int cpu_id, struct cpu_pwr_ctrl *pwr_ctrl)
+{
+	unsigned int val = is_core_power_status_on(cpu_id);
+
+	CPU_PM_ASSERT(pwr_ctrl);
+
+	mmio_clrbits_32(pwr_ctrl->pwpr, RESETPWRON_CONFIG);
+	if (val == 0) {
+		/*
+		 * Set to 0 after BIG VPROC bulk powered on (configure in MCUPM) and
+		 * before big core power-on sequence.
+		 */
+		if (cpu_id >= PLAT_CPU_PM_B_BUCK_ISO_ID) {
+			mmio_write_32(DREQ20_BIG_VPROC_ISO, 0);
+		}
+
+		mmio_setbits_32(pwr_ctrl->pwpr, PWR_RST_B);
+		dsbsy();
+
+		/* set mp0_spmc_pwr_on_cpuX = 1 */
+		mmio_setbits_32(pwr_ctrl->pwpr, PWR_ON);
+
+		val = 0;
+		while (is_core_power_status_on(cpu_id) == 0) {
+			DO_SMP_CORE_ON_WAIT_TIMEOUT(val);
+			mmio_clrbits_32(pwr_ctrl->pwpr, PWR_ON);
+			mmio_setbits_32(pwr_ctrl->pwpr, PWR_ON);
+		}
+	} else {
+		INFO("[%s:%d] - core_%u haven been power on\n", __func__, __LINE__, cpu_id);
+	}
+
+	return MTK_CPUPM_E_OK;
+}
+
+int mt_smp_power_core_off(struct cpu_pwr_ctrl *pwr_ctrl)
+{
+	/* set mp0_spmc_pwr_on_cpuX = 1 */
+	mmio_clrbits_32(pwr_ctrl->pwpr, PWR_ON);
+	return MTK_CPUPM_E_OK;
+}
+
+void mt_smp_init(void)
+{
+	/* clear RESETPWRON_CONFIG of mcusys/cluster/core0 */
+	mmio_clrbits_32(SPM_MCUSYS_PWR_CON, RESETPWRON_CONFIG);
+	mmio_clrbits_32(SPM_MP0_CPUTOP_PWR_CON, RESETPWRON_CONFIG);
+}
diff --git a/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_smp.h b/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_smp.h
new file mode 100644
index 0000000..4c2f8d2
--- /dev/null
+++ b/plat/mediatek/drivers/cpu_pm/cpcv3_2/mt_smp.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MT_SMP_H
+#define MT_SMP_H
+
+#include <lib/mmio.h>
+#include <platform_def.h>
+
+#define CPU_PWR_STATUS			(MCUCFG_BASE + 0xA840)
+
+#define SMP_CORE_TIMEOUT_MAX		(50000)
+#define DO_SMP_CORE_ON_WAIT_TIMEOUT(k_cnt) ({ \
+		CPU_PM_ASSERT(k_cnt < SMP_CORE_TIMEOUT_MAX); \
+		k_cnt++; udelay(1); })
+
+void mt_smp_core_init_arch(unsigned int cluster, unsigned int cpu, int arm64,
+			   struct cpu_pwr_ctrl *pwr_ctrl);
+void mt_smp_core_bootup_address_set(struct cpu_pwr_ctrl *pwr_ctrl, uintptr_t entry);
+int mt_smp_power_core_on(unsigned int cpu_id, struct cpu_pwr_ctrl *pwr_ctrl);
+int mt_smp_power_core_off(struct cpu_pwr_ctrl *pwr_ctrl);
+void mt_smp_init(void);
+
+#endif /* MT_SMP_H */
diff --git a/plat/mediatek/drivers/cpu_pm/cpcv3_2/rules.mk b/plat/mediatek/drivers/cpu_pm/cpcv3_2/rules.mk
new file mode 100644
index 0000000..858cf38
--- /dev/null
+++ b/plat/mediatek/drivers/cpu_pm/cpcv3_2/rules.mk
@@ -0,0 +1,19 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := cpcv${CONFIG_MTK_CPU_PM_ARCH}
+
+LOCAL_SRCS-y := ${LOCAL_DIR}/mt_cpu_pm.c ${LOCAL_DIR}/mt_cpu_pm_cpc.c
+
+LOCAL_SRCS-$(CPU_PM_TINYSYS_SUPPORT) += ${LOCAL_DIR}/mt_cpu_pm_mbox.c
+LOCAL_SRCS-$(CONFIG_MTK_SMP_EN) += ${LOCAL_DIR}/mt_smp.c
+
+$(eval $(call add_defined_option,CPU_PM_TINYSYS_SUPPORT))
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
+
diff --git a/plat/mediatek/drivers/cpu_pm/rules.mk b/plat/mediatek/drivers/cpu_pm/rules.mk
new file mode 100644
index 0000000..8df4f21
--- /dev/null
+++ b/plat/mediatek/drivers/cpu_pm/rules.mk
@@ -0,0 +1,13 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := cpu_pm
+
+SUB_RULES-${CONFIG_MTK_CPU_PM_SUPPORT} := $(LOCAL_DIR)/cpcv${CONFIG_MTK_CPU_PM_ARCH}
+
+$(eval $(call INCLUDE_MAKEFILE,$(SUB_RULES-y)))