feat(plat/mediatek/mt8186): add MCDI drivers

Add MCDI related drivers to handle CPU powered on/off in CPU suspend.

TEST=build pass
BUG=b:202871018

Change-Id: I85aaaf3a0e992a39d17c58f3d9d5ff1b5770f748
Signed-off-by: Garmin.Chang <Garmin.Chang@mediatek.com>

12 files changed, 978 insertions, 0 deletions

diff --git a/plat/mediatek/mt8186/drivers/mcdi/build.mk b/plat/mediatek/mt8186/drivers/mcdi/build.mk
new file mode 100644
index 000000000..45acb5d1c
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/build.mk
@@ -0,0 +1,31 @@
+#
+# Copyright (c) 2021, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+MCDI_TINYSYS_TYPE = sspm
+MCDI_TINYSYS_MBOX_TYPE = share_sram
+
+CUR_MCDI_FOLDER = ${MTK_PLAT_SOC}/drivers/mcdi
+
+BL31_MT_LPM_PLAT_CFLAGS += -I${CUR_MCDI_FOLDER}/
+
+BL31_MT_LPM_PLAT_SOURCE += \
+	${CUR_MCDI_FOLDER}/mt_cpu_pm.c	\
+	${CUR_MCDI_FOLDER}/mt_cpu_pm_cpc.c \
+	${CUR_MCDI_FOLDER}/mt_mcdi.c	\
+	${CUR_MCDI_FOLDER}/mt_lp_irqremain.c
+
+
+ifeq ($(MCDI_TINYSYS_TYPE), sspm)
+BL31_MT_LPM_PLAT_CFLAGS += -DMCDI_TINYSYS_SSPM
+BL31_MT_LPM_PLAT_SOURCE += ${CUR_MCDI_FOLDER}/mt_cpu_pm_mbox_sspm.c
+else
+BL31_MT_LPM_PLAT_CFLAGS += -DMCDI_TINYSYS_MCUPM
+BL31_MT_LPM_PLAT_SOURCE += ${CUR_MCDI_FOLDER}/mt_cpu_pm_mbox.c
+endif
+
+ifeq ($(MCDI_TINYSYS_MBOX_TYPE), share_sram)
+BL31_MT_LPM_PLAT_CFLAGS += -DMCDI_TINYSYS_MBOX_SHARE_SRAM
+endif
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 000000000..c6c2e3825
--- /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 000000000..83a7a5359
--- /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 000000000..2b0f0716b
--- /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 000000000..488b1d1b2
--- /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_cpu_pm_mbox.c b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox.c
new file mode 100644
index 000000000..789fafb69
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox.c
@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <mmio.h>
+#include <mt_cpu_pm_mbox.h>
+#include <platform_def.h>
+#include <sspm_reg.h>
+
+#define MCUPM_MBOX_3_BASE	(MTK_MCUPM_SRAM_BASE + 0xFCE0)
+
+#define _sspm_mbox_write(id, val) \
+		mmio_write_32(SSPM_MBOX_3_BASE + 4 * (id), val)
+#define _sspm_mbox_read(id) \
+		mmio_read_32(SSPM_MBOX_3_BASE + 4 * (id))
+
+#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))
+
+
+#define MCUPM_MBOX_OFFSET_PDN		(0x0C55FDA8)
+#define MCUPM_POWER_DOWN		(0x4D50444E)
+
+void mtk_set_sspm_lp_cmd(void *buf, unsigned int size)
+{
+	unsigned int *p = (unsigned int *)buf;
+	int i;
+
+	for (i = 0; i < size; i++) {
+		_sspm_mbox_write(SSPM_MBOX_SPM_CMD + i, p[i]);
+	}
+}
+
+void mtk_clr_sspm_lp_cmd(unsigned int size)
+{
+	int i;
+
+	for (i = 0; i < size; i++) {
+		_sspm_mbox_write(SSPM_MBOX_SPM_CMD + i, 0);
+	}
+}
+
+void mtk_set_cpu_pm_pll_mode(unsigned int mode)
+{
+	if (mode < NF_MCUPM_ARMPLL_MODE) {
+		_mcupm_mbox_write(MCUPM_MBOX_ARMPLL_MODE, mode);
+	}
+}
+
+int mtk_get_cpu_pm_pll_mode(void)
+{
+	return _mcupm_mbox_read(MCUPM_MBOX_ARMPLL_MODE);
+}
+
+void mtk_set_cpu_pm_buck_mode(unsigned int mode)
+{
+	if (mode < NF_MCUPM_BUCK_MODE) {
+		_mcupm_mbox_write(MCUPM_MBOX_BUCK_MODE, mode);
+	}
+}
+
+int mtk_get_cpu_pm_buck_mode(void)
+{
+	return _mcupm_mbox_read(MCUPM_MBOX_BUCK_MODE);
+}
+
+void mtk_set_cpu_pm_preffered_cpu(unsigned int cpuid)
+{
+	return _mcupm_mbox_read(MCUPM_MBOX_WAKEUP_CPU);
+}
+
+int mtk_set_cpu_pm_mbox_addr(uint64_t phy_addr)
+{
+	return 0;
+}
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox.h b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox.h
new file mode 100644
index 000000000..63a43af4b
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox.h
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef __MT_CPU_PM_MBOX_H__
+#define __MT_CPU_PM_MBOX_H__
+
+/* SSPM Mbox */
+/* AP Write */
+#define SSPM_MBOX_SPM_CMD	(0U)
+#define SSPM_MBOX_SPM_ARGS1	(1U)
+#define SSPM_MBOX_SPM_ARGS2	(2U)
+#define SSPM_MBOX_SPM_ARGS3	(3U)
+#define SSPM_MBOX_SPM_ARGS4	(4U)
+#define SSPM_MBOX_SPM_ARGS5	(5U)
+#define SSPM_MBOX_SPM_ARGS6	(6U)
+#define SSPM_MBOX_SPM_ARGS7	(7U)
+#define SSPM_MBOX_AP_READY	(17U)
+
+#define SSPM_MBOX_SPM_CMD_SIZE	(8U)
+
+void mtk_set_sspm_lp_cmd(void *buf, unsigned int size);
+void mtk_clr_sspm_lp_cmd(unsigned int size);
+
+/* MCUPM Mbox */
+/* AP Write */
+#define MCUPM_MBOX_AP_READY		(0U)
+#define MCUPM_MBOX_RESERVED_1		(1U)
+#define MCUPM_MBOX_RESERVED_2		(2U)
+#define MCUPM_MBOX_RESERVED_3		(3U)
+#define MCUPM_MBOX_PWR_CTRL_EN		(4U)
+#define MCUPM_MBOX_L3_CACHE_MODE	(5U)
+#define MCUPM_MBOX_BUCK_MODE		(6U)
+#define MCUPM_MBOX_ARMPLL_MODE		(7U)
+/* AP Read */
+#define MCUPM_MBOX_TASK_STA		(8U)
+#define MCUPM_MBOX_RESERVED_9		(9U)
+#define MCUPM_MBOX_RESERVED_10		(10U)
+#define MCUPM_MBOX_RESERVED_11		(11U)
+/* CPC mode - Read/Write */
+#define MCUPM_MBOX_WAKEUP_CPU		(12U)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_PWR_CTRL_EN (4) */
+#define MCUPM_MCUSYS_CTRL		(1U << 0)
+#define MCUPM_BUCK_CTRL			(1U << 1)
+#define MCUPM_ARMPLL_CTRL		(1U << 2)
+#define MCUPM_PWR_CTRL_MASK		((1U << 3) - 1U)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_L3_CACHE_MODE (5) */
+#define MCUPM_L3_OFF_MODE		(0U) /* default */
+#define MCUPM_L3_DORMANT_MODE		(1U)
+#define NF_MCUPM_L3_MODE		(2U)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_BUCK_MODE (6) */
+#define MCUPM_BUCK_NORMAL_MODE		(0U) /* default */
+#define MCUPM_BUCK_LP_MODE		(1U)
+#define MCUPM_BUCK_OFF_MODE		(2U)
+#define NF_MCUPM_BUCK_MODE		(3U)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_ARMPLL_MODE (7) */
+#define MCUPM_ARMPLL_ON			(0U) /* default */
+#define MCUPM_ARMPLL_GATING		(1U)
+#define MCUPM_ARMPLL_OFF		(2U)
+#define NF_MCUPM_ARMPLL_MODE		(3U)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_TASK_STA (9) */
+#define MCUPM_TASK_UNINIT		(0U)
+#define MCUPM_TASK_INIT			(1U)
+#define MCUPM_TASK_INIT_FINISH		(2U)
+#define MCUPM_TASK_WAIT			(3U)
+#define MCUPM_TASK_RUN			(4U)
+#define MCUPM_TASK_PAUSE		(5U)
+
+void mtk_set_cpu_pm_pll_mode(unsigned int mode);
+int mtk_get_cpu_pm_pll_mode(void);
+void mtk_set_cpu_pm_buck_mode(unsigned int mode);
+int mtk_get_cpu_pm_buck_mode(void);
+void mtk_set_cpu_pm_preffered_cpu(unsigned int cpuid);
+int mtk_set_cpu_pm_mbox_addr(uint64_t phy_addr);
+
+#endif
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox_sspm.c b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox_sspm.c
new file mode 100644
index 000000000..34c281dc8
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox_sspm.c
@@ -0,0 +1,90 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <mmio.h>
+#include <mt_cpu_pm.h>
+#include <mt_cpu_pm_mbox.h>
+#include <platform_def.h>
+#include <sspm_reg.h>
+
+#ifdef MCDI_TINYSYS_MBOX_SHARE_SRAM
+struct cpu_pm_mbox {
+	unsigned int ap_ready;
+	unsigned int reserved1;
+	unsigned int reserved2;
+	unsigned int reserved3;
+	unsigned int pwr_ctrl_en;
+	unsigned int l3_cache_mode;
+	unsigned int buck_mode;
+	unsigned int armpll_mode;
+	unsigned int task_sta;
+	unsigned int reserved9;
+	unsigned int reserved10;
+	unsigned int reserved11;
+	unsigned int wakeup_cpu;
+};
+
+struct cpu_pm_mbox *_cpu_pm_box = (struct cpu_pm_mbox *)SSPM_MBOX_3_BASE;
+#endif
+
+void mtk_set_cpu_pm_pll_mode(unsigned int mode)
+{
+#ifdef MCDI_TINYSYS_MBOX_SHARE_SRAM
+	if (_cpu_pm_box) {
+		_cpu_pm_box->armpll_mode = mode;
+	}
+#endif
+}
+
+int mtk_get_cpu_pm_pll_mode(void)
+{
+#ifdef MCDI_TINYSYS_MBOX_SHARE_SRAM
+	if (!_cpu_pm_box) {
+		return 0;
+	}
+	return _cpu_pm_box->armpll_mode;
+#endif
+}
+
+void mtk_set_cpu_pm_buck_mode(unsigned int mode)
+{
+#ifdef MCDI_TINYSYS_MBOX_SHARE_SRAM
+	if (_cpu_pm_box) {
+		_cpu_pm_box->buck_mode = mode;
+	}
+#endif
+}
+
+int mtk_get_cpu_pm_buck_mode(void)
+{
+#ifdef MCDI_TINYSYS_MBOX_SHARE_SRAM
+	if (!_cpu_pm_box) {
+		return 0;
+	}
+	return _cpu_pm_box->buck_mode;
+#endif
+}
+
+void mtk_set_cpu_pm_preffered_cpu(unsigned int cpuid)
+{
+#ifdef MCDI_TINYSYS_MBOX_SHARE_SRAM
+	if (_cpu_pm_box) {
+		_cpu_pm_box->wakeup_cpu = cpuid;
+	}
+#endif
+}
+
+int mtk_set_cpu_pm_mbox_addr(uint64_t phy_addr)
+{
+#ifdef MCDI_TINYSYS_MBOX_SHARE_SRAM
+	if (_cpu_pm_box || (phy_addr == 0)) {
+		return -1;
+	}
+
+	_cpu_pm_box = (struct cpu_pm_mbox *)(MTK_SSPM_BASE + phy_addr);
+#endif
+	return 0;
+}
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 000000000..010361208
--- /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 000000000..0e6444ade
--- /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/plat_pm.c b/plat/mediatek/mt8186/plat_pm.c
index b014297b2..607470f89 100644
--- a/plat/mediatek/mt8186/plat_pm.c
+++ b/plat/mediatek/mt8186/plat_pm.c
@@ -361,6 +361,7 @@ int plat_setup_psci_ops(uintptr_t sec_entrypoint,
 	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/platform.mk b/plat/mediatek/mt8186/platform.mk
index 31a030a85..217fd7ad5 100644
--- a/plat/mediatek/mt8186/platform.mk
+++ b/plat/mediatek/mt8186/platform.mk
@@ -14,6 +14,7 @@ PLAT_INCLUDES := -I${MTK_PLAT}/common/                            \
                  -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/                  \
@@ -54,6 +55,9 @@ BL31_SOURCES += common/desc_image_load.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                 \