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

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

14 files changed, 1221 insertions, 0 deletions

diff --git a/plat/mediatek/common/common_config.mk b/plat/mediatek/common/common_config.mk
new file mode 100644
index 0000000..300242b
--- /dev/null
+++ b/plat/mediatek/common/common_config.mk
@@ -0,0 +1,33 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# indicate the reset vector address can be programmed
+PROGRAMMABLE_RESET_ADDRESS := 1
+COLD_BOOT_SINGLE_CPU := 1
+# Build flag to include AArch32 registers in cpu context save and restore during
+# world switch. This flag must be set to 0 for AArch64-only platforms.
+CTX_INCLUDE_AARCH32_REGS := 0
+PLAT_XLAT_TABLES_DYNAMIC := 1
+# enable this definition to print irq dump status in tf-a
+GIC_DEBUG := 0
+# Enable stack protector.
+# Allowed values are "all", "strong", "default" and "none"
+ENABLE_STACK_PROTECTOR := strong
+# AMU, Kernel will access amuserenr_el0 if PE supported
+# Firmware _must_ implement AMU support
+ENABLE_FEAT_AMU := 2
+VENDOR_EXTEND_PUBEVENT_ENABLE := 1
+
+# MTK define options
+MTK_BL33_IS_64BIT := 0
+MTK_ADAPTED := 1
+
+# MTK module config
+CONFIG_MTK_INTERRUPT := y
+CONFIG_MTK_UART := y
+
+# UART baudrate
+UART_BAUDRATE := 921600
diff --git a/plat/mediatek/common/coreboot_config.mk b/plat/mediatek/common/coreboot_config.mk
new file mode 100644
index 0000000..59d18e8
--- /dev/null
+++ b/plat/mediatek/common/coreboot_config.mk
@@ -0,0 +1,15 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# indicate the reset vector address can be programmed
+PROGRAMMABLE_RESET_ADDRESS := 1
+COLD_BOOT_SINGLE_CPU := 1
+# Build flag to include AArch32 registers in cpu context save and restore during
+# world switch. This flag must be set to 0 for AArch64-only platforms.
+CTX_INCLUDE_AARCH32_REGS := 0
+PLAT_XLAT_TABLES_DYNAMIC := 1
+
+VENDOR_EXTEND_PUBEVENT_ENABLE := 1
diff --git a/plat/mediatek/common/lpm/mt_lp_api.c b/plat/mediatek/common/lpm/mt_lp_api.c
new file mode 100644
index 0000000..2a1da6a
--- /dev/null
+++ b/plat/mediatek/common/lpm/mt_lp_api.c
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2023, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lpm/mt_lp_api.h>
+
+int mt_audio_update(int type)
+{
+	int ret, val;
+
+	switch (type) {
+	case AUDIO_AFE_ENTER:
+	case AUDIO_AFE_LEAVE:
+		val = (type == AUDIO_AFE_ENTER) ? 1 : 0;
+		ret = mt_lp_rm_do_update(-1, PLAT_RC_IS_FMAUDIO, &val);
+		break;
+	case AUDIO_DSP_ENTER:
+	case AUDIO_DSP_LEAVE:
+		val = (type == AUDIO_DSP_ENTER) ? 1 : 0;
+		ret = mt_lp_rm_do_update(-1, PLAT_RC_IS_ADSP, &val);
+		break;
+	default:
+		ret = -1;
+		break;
+	}
+
+	return ret;
+}
+
+int mtk_usb_update(int type)
+{
+	int ret, val;
+
+	switch (type) {
+	case LPM_USB_ENTER:
+	case LPM_USB_LEAVE:
+		val = (type == LPM_USB_ENTER) ? 1 : 0;
+		ret = mt_lp_rm_do_update(-1, PLAT_RC_IS_USB_INFRA, &val);
+		break;
+	default:
+		ret = -1;
+		break;
+	}
+
+	return ret;
+}
diff --git a/plat/mediatek/common/lpm/mt_lp_rm.c b/plat/mediatek/common/lpm/mt_lp_rm.c
new file mode 100644
index 0000000..9f07968
--- /dev/null
+++ b/plat/mediatek/common/lpm/mt_lp_rm.c
@@ -0,0 +1,162 @@
+/*
+ * Copyright (c) 2020-2023, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+#include <lpm/mt_lp_rm.h>
+
+struct platform_mt_resource_manager {
+	unsigned int count;
+	struct mt_resource_manager *plat_rm;
+};
+
+static struct platform_mt_resource_manager plat_mt_rm;
+
+int mt_lp_rm_register(struct mt_resource_manager *rm)
+{
+	unsigned int i;
+	struct mt_resource_constraint *const *rc;
+
+	if ((rm == NULL) || (rm->consts == NULL) ||
+	    (plat_mt_rm.plat_rm != NULL)) {
+		return MT_RM_STATUS_BAD;
+	}
+
+	for (i = 0U, rc = rm->consts; *rc != NULL; i++, rc++) {
+		if ((*rc)->init != NULL) {
+			(*rc)->init();
+		}
+	}
+
+	plat_mt_rm.plat_rm = rm;
+	plat_mt_rm.count = i;
+
+	return MT_RM_STATUS_OK;
+}
+
+int mt_lp_rm_reset_constraint(unsigned int idx, unsigned int cpuid, int stateid)
+{
+	struct mt_resource_constraint const *rc = NULL;
+
+	if ((plat_mt_rm.plat_rm == NULL) || (idx >= plat_mt_rm.count)) {
+		return MT_RM_STATUS_BAD;
+	}
+
+	rc = plat_mt_rm.plat_rm->consts[idx];
+
+	if ((rc == NULL) || (rc->reset == NULL)) {
+		return MT_RM_STATUS_BAD;
+	}
+
+	return rc->reset(cpuid, stateid);
+}
+
+int mt_lp_rm_get_status(unsigned int type, void *priv)
+{
+	int res = 0;
+	struct mt_resource_constraint *const *con;
+	struct mt_resource_manager *rm = plat_mt_rm.plat_rm;
+
+	if ((rm == NULL) || (type >= PLAT_RC_MAX)) {
+		return -1;
+	}
+
+	for (con = rm->consts; *con != NULL; con++) {
+		if ((*con)->get_status == NULL) {
+			continue;
+		}
+		res = (*con)->get_status(type, priv);
+		if (res == MT_RM_STATUS_STOP) {
+			break;
+		}
+	}
+
+	return res;
+}
+
+int mt_lp_rm_do_constraint(unsigned int constraint_id, unsigned int cpuid, int stateid)
+{
+	int res = MT_RM_STATUS_BAD;
+	struct mt_resource_constraint const *rc;
+	struct mt_resource_manager *rm = plat_mt_rm.plat_rm;
+
+	if ((rm == NULL) || (constraint_id >= plat_mt_rm.count)) {
+		return res;
+	}
+
+	rc = rm->consts[constraint_id];
+	if ((rc != NULL) && (rc->run != NULL)) {
+		res = rc->run(cpuid, stateid);
+	}
+
+	return res;
+}
+
+int mt_lp_rm_find_constraint(unsigned int idx, unsigned int cpuid,
+			     int stateid, void *priv)
+{
+	unsigned int i;
+	int res = MT_RM_STATUS_BAD;
+	struct mt_resource_constraint *const *rc;
+	struct mt_resource_manager *rm = plat_mt_rm.plat_rm;
+
+	if ((rm == NULL) || (idx >= plat_mt_rm.count)) {
+		return res;
+	}
+
+	/* If subsys clk/mtcmos is on, add block-resource-off flag */
+	if (rm->update != NULL) {
+		res = rm->update(rm->consts, plat_mt_rm.count, stateid, priv);
+		if (res != 0) {
+			return MT_RM_STATUS_BAD;
+		}
+	}
+
+	res = MT_RM_STATUS_BAD;
+	for (i = idx, rc = (rm->consts + idx); *rc != NULL; i++, rc++) {
+		if (((*rc)->is_valid != NULL) &&
+		    ((*rc)->is_valid(cpuid, stateid))) {
+			res = i;
+			break;
+		}
+	}
+
+	return res;
+}
+
+int mt_lp_rm_find_and_run_constraint(unsigned int idx, unsigned int cpuid,
+				     int stateid, void *priv)
+{
+	int res = MT_RM_STATUS_BAD;
+
+	res = mt_lp_rm_find_constraint(idx, cpuid, stateid, priv);
+	if (res != MT_RM_STATUS_BAD) {
+		mt_lp_rm_do_constraint(res, cpuid, stateid);
+	}
+
+	return res;
+}
+
+int mt_lp_rm_do_update(int stateid, int type, void const *p)
+{
+	int res = MT_RM_STATUS_BAD;
+	struct mt_resource_constraint *const *rc;
+	struct mt_resource_manager *rm = plat_mt_rm.plat_rm;
+
+	if (rm == NULL) {
+		return res;
+	}
+
+	for (rc = rm->consts; *rc != NULL; rc++) {
+		if ((*rc)->update != NULL) {
+			res = (*rc)->update(stateid, type, p);
+			if (res != MT_RM_STATUS_OK) {
+				break;
+			}
+		}
+	}
+
+	return res;
+}
diff --git a/plat/mediatek/common/lpm/mt_lp_rq.c b/plat/mediatek/common/lpm/mt_lp_rq.c
new file mode 100644
index 0000000..7b83fed
--- /dev/null
+++ b/plat/mediatek/common/lpm/mt_lp_rq.c
@@ -0,0 +1,204 @@
+/*
+ * Copyright (c) 2023, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <string.h>
+#include <common/debug.h>
+#include <drivers/console.h>
+#include <lib/spinlock.h>
+#include <lpm/mt_lp_rqm.h>
+
+struct mt_lp_res_req_m {
+	unsigned int uname[MT_LP_RQ_USER_MAX];
+	unsigned int user_num;
+	unsigned int user_valid;
+	unsigned int resource_num;
+	unsigned int generic_resource_req;
+	unsigned int flag;
+	struct mt_resource_req_manager *plat_rqm;
+};
+
+static struct mt_lp_res_req_m plat_mt_rqm;
+static spinlock_t mt_lp_rq_lock;
+
+static int mt_lp_resource_request(struct mt_lp_resource_user *this, unsigned int resource)
+{
+	int i;
+	struct mt_lp_res_req *const *rs;
+
+	if ((this == NULL) || (resource == 0) || (resource > MT_LP_RQ_ALL)) {
+		ERROR("invalid request(%x)\n", resource);
+		return MT_LP_RQ_STA_BAD;
+	}
+
+	spin_lock(&mt_lp_rq_lock);
+
+	rs = (plat_mt_rqm.plat_rqm)->res;
+	for (i = 0; i < plat_mt_rqm.resource_num; i++) {
+		if ((resource & rs[i]->res_id) != 0) {
+			rs[i]->res_usage |= this->umask;
+		}
+	}
+
+	plat_mt_rqm.flag = MT_LP_RQ_FLAG_NEED_UPDATE;
+	spin_unlock(&mt_lp_rq_lock);
+
+	return MT_LP_RQ_STA_OK;
+}
+
+static int mt_lp_resource_release(struct mt_lp_resource_user *this)
+{
+	int i;
+	struct mt_lp_res_req *const *rs;
+
+	if (this == NULL) {
+		return MT_LP_RQ_STA_BAD;
+	}
+
+	spin_lock(&mt_lp_rq_lock);
+
+	rs = (plat_mt_rqm.plat_rqm)->res;
+	for (i = 0; i < plat_mt_rqm.resource_num; i++) {
+		rs[i]->res_usage &= ~(this->umask);
+	}
+
+	plat_mt_rqm.flag = MT_LP_RQ_FLAG_NEED_UPDATE;
+	spin_unlock(&mt_lp_rq_lock);
+
+	return MT_LP_RQ_STA_OK;
+}
+
+int mt_lp_resource_request_manager_register(struct mt_resource_req_manager *rqm)
+{
+	unsigned int count;
+	struct mt_lp_res_req *const *rs;
+
+	if ((rqm == NULL) || (rqm->res == NULL) || (plat_mt_rqm.plat_rqm != NULL)) {
+		return MT_LP_RQ_STA_BAD;
+	}
+
+	rs = rqm->res;
+	count = 0;
+	while (*rs != NULL) {
+		count++;
+		rs++;
+	}
+
+	plat_mt_rqm.plat_rqm = rqm;
+	plat_mt_rqm.resource_num = count;
+
+	return MT_LP_RQ_STA_OK;
+}
+
+int mt_lp_resource_user_register(char *user, struct mt_lp_resource_user *ru)
+{
+	int i, len;
+	unsigned int uname;
+
+	if ((plat_mt_rqm.plat_rqm == NULL) || (plat_mt_rqm.user_num >= MT_LP_RQ_USER_MAX) ||
+	    (user == NULL)) {
+		ru->uid = MT_LP_RQ_USER_INVALID;
+		ru->umask = 0;
+		ru->request = NULL;
+		ru->release = NULL;
+		ERROR("rqm register user invalid\n");
+		return MT_LP_RQ_STA_BAD;
+	}
+
+	len = strnlen(user, MT_LP_RQ_USER_NAME_LEN);
+
+	uname = 0;
+	for (i = 0; i < len; i++) {
+		uname |= (user[i] << (MT_LP_RQ_USER_CHAR_U * i));
+	}
+
+	spin_lock(&mt_lp_rq_lock);
+	i = plat_mt_rqm.user_num;
+	plat_mt_rqm.user_num += 1;
+	plat_mt_rqm.uname[i] = uname;
+	plat_mt_rqm.user_valid |= BIT(i);
+	spin_unlock(&mt_lp_rq_lock);
+
+	ru->umask = BIT(i);
+	ru->uid = i;
+	ru->request = mt_lp_resource_request;
+	ru->release = mt_lp_resource_release;
+	INFO("%s register by %s, uid = %d\n", __func__, user, ru->uid);
+
+	return MT_LP_RQ_STA_OK;
+}
+
+int mt_lp_rq_get_status(int type, void *p)
+{
+	int i;
+	unsigned int update_sta;
+	struct mt_lp_res_req *const *rs;
+	struct resource_req_status *rq_sta = (struct resource_req_status *)p;
+
+	if (plat_mt_rqm.flag != 0) {
+		spin_lock(&mt_lp_rq_lock);
+
+		update_sta = 0;
+		rs = (plat_mt_rqm.plat_rqm)->res;
+		for (i = 0; i < plat_mt_rqm.resource_num; i++) {
+			update_sta |= ((rs[i]->res_usage & plat_mt_rqm.user_valid) != 0) ?
+				      rs[i]->res_rq : 0;
+		}
+
+		plat_mt_rqm.generic_resource_req = update_sta;
+		plat_mt_rqm.flag = MT_LP_RQ_FLAG_DONE;
+		spin_unlock(&mt_lp_rq_lock);
+	}
+
+	switch (type) {
+	case PLAT_RQ_REQ_USAGE:
+		rs = (plat_mt_rqm.plat_rqm)->res;
+		rq_sta->val = (rq_sta->id < plat_mt_rqm.resource_num) ?
+			      rs[rq_sta->id]->res_usage : plat_mt_rqm.generic_resource_req;
+		break;
+	case PLAT_RQ_USER_NUM:
+		rq_sta->val = plat_mt_rqm.user_num;
+		break;
+	case PLAT_RQ_USER_VALID:
+		rq_sta->val = plat_mt_rqm.user_valid;
+		break;
+	case PLAT_RQ_PER_USER_NAME:
+		rq_sta->val = (rq_sta->id < plat_mt_rqm.user_num) ?
+			      plat_mt_rqm.uname[rq_sta->id] : 0;
+		break;
+	case PLAT_RQ_REQ_NUM:
+		rq_sta->val = plat_mt_rqm.resource_num;
+		break;
+	default:
+		break;
+	}
+
+	return MT_LP_RQ_STA_OK;
+}
+
+int mt_lp_rq_update_status(int type, void *p)
+{
+	unsigned int user_mask;
+	struct resource_req_status *rq_sta = (struct resource_req_status *)p;
+
+	switch (type) {
+	case PLAT_RQ_USER_VALID:
+		if (rq_sta->id < plat_mt_rqm.user_num) {
+			user_mask = BIT(rq_sta->id);
+			spin_lock(&mt_lp_rq_lock);
+			plat_mt_rqm.user_valid = (rq_sta->val == 0) ?
+						 (plat_mt_rqm.user_valid & ~(user_mask)) :
+						 (plat_mt_rqm.user_valid | user_mask);
+			plat_mt_rqm.flag = MT_LP_RQ_FLAG_NEED_UPDATE;
+			spin_unlock(&mt_lp_rq_lock);
+		}
+		break;
+	default:
+		break;
+	}
+
+	return MT_LP_RQ_STA_OK;
+}
diff --git a/plat/mediatek/common/lpm/rules.mk b/plat/mediatek/common/lpm/rules.mk
new file mode 100644
index 0000000..eb68e03
--- /dev/null
+++ b/plat/mediatek/common/lpm/rules.mk
@@ -0,0 +1,17 @@
+#
+# Copyright (c) 2023, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := lpm
+
+LOCAL_SRCS-y := $(LOCAL_DIR)/mt_lp_api.c
+LOCAL_SRCS-y += $(LOCAL_DIR)/mt_lp_rm.c
+LOCAL_SRCS-y += $(LOCAL_DIR)/mt_lp_rq.c
+
+PLAT_INCLUDES += -I${LOCAL_DIR}
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/common/mtk_bl31_setup.c b/plat/mediatek/common/mtk_bl31_setup.c
new file mode 100644
index 0000000..7c9db8b
--- /dev/null
+++ b/plat/mediatek/common/mtk_bl31_setup.c
@@ -0,0 +1,175 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <arch.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/generic_delay_timer.h>
+#if XLAT_TABLES_LIB_V2 && PLAT_XLAT_TABLES_DYNAMIC
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#endif
+#include <plat/common/platform.h>
+
+#if COREBOOT
+#include <common/desc_image_load.h>
+
+#include <drivers/ti/uart/uart_16550.h>
+#include <lib/coreboot.h>
+#include <plat_params.h>
+#endif
+
+/* MTK headers */
+#if MTK_SIP_KERNEL_BOOT_ENABLE
+#include <cold_boot.h>
+#endif
+#include <lib/mtk_init/mtk_init.h>
+#include <mtk_mmap_pool.h>
+
+IMPORT_SYM(uintptr_t, __RW_START__, RW_START);
+IMPORT_SYM(uintptr_t, __DATA_START__, DATA_START);
+
+#if COREBOOT
+static entry_point_info_t bl32_ep_info;
+static entry_point_info_t bl33_ep_info;
+
+/*******************************************************************************
+ * Return a pointer to the 'entry_point_info' structure of the next image for
+ * the security state specified. BL33 corresponds to the non-secure image type
+ * while BL32 corresponds to the secure image type. A NULL pointer is returned
+ * if the image does not exist.
+ ******************************************************************************/
+entry_point_info_t *bl31_plat_get_next_image_ep_info(uint32_t type)
+{
+	entry_point_info_t *next_image_info;
+
+	next_image_info = (type == NON_SECURE) ? &bl33_ep_info : &bl32_ep_info;
+	assert(next_image_info->h.type == PARAM_EP);
+
+	/* None of the images on this platform can have 0x0 as the entrypoint */
+	if (next_image_info->pc) {
+		return next_image_info;
+	} else {
+		return NULL;
+	}
+}
+#else
+#ifndef MTK_BL31_AS_BL2
+static struct mtk_bl31_fw_config bl31_fw_config;
+#else
+struct mtk_bl31_fw_config bl31_fw_config;
+#endif
+/* In order to be accessed after MMU enable */
+static struct mtk_bl_param_t bl_param_clone;
+
+void *get_mtk_bl31_fw_config(int index)
+{
+	void *arg = NULL;
+
+	switch (index) {
+	case BOOT_ARG_FROM_BL2:
+		arg = bl31_fw_config.from_bl2;
+		break;
+	case BOOT_ARG_SOC_FW_CONFIG:
+		arg = bl31_fw_config.soc_fw_config;
+		break;
+	case BOOT_ARG_HW_CONFIG:
+		arg = bl31_fw_config.hw_config;
+		break;
+	case BOOT_ARG_RESERVED:
+		arg = bl31_fw_config.reserved;
+		break;
+	default:
+		WARN("Fail to get boot arg, index:%d", index);
+		break;
+	}
+	return arg;
+}
+#endif
+/*****************************************************************************
+ * Perform the very early platform specific architectural setup shared between
+ * ARM standard platforms. This only does basic initialization. Later
+ * architectural setup (bl31_arch_setup()) does not do anything platform
+ * specific.
+ ******************************************************************************/
+void bl31_early_platform_setup2(u_register_t from_bl2,
+				u_register_t soc_fw_config,
+				u_register_t hw_config, u_register_t plat_params_from_bl2)
+
+{
+#if COREBOOT
+	static console_t console;
+
+	params_early_setup(soc_fw_config);
+	if (coreboot_serial.type) {
+		console_16550_register(coreboot_serial.baseaddr,
+				       coreboot_serial.input_hertz,
+				       coreboot_serial.baud,
+				       &console);
+	}
+	bl31_params_parse_helper(from_bl2, &bl32_ep_info, &bl33_ep_info);
+#else
+	struct mtk_bl_param_t *p_mtk_bl_param = (struct mtk_bl_param_t *)from_bl2;
+
+	if (p_mtk_bl_param == NULL) {
+		ERROR("from_bl2 should not be NULL\n");
+		panic();
+	}
+	memcpy(&bl_param_clone, p_mtk_bl_param, sizeof(struct mtk_bl_param_t));
+	bl31_fw_config.from_bl2 = (void *)&bl_param_clone;
+	bl31_fw_config.soc_fw_config = (void *)soc_fw_config;
+	bl31_fw_config.hw_config = (void *)hw_config;
+	bl31_fw_config.reserved = (void *)plat_params_from_bl2;
+#endif
+
+	INFO("MTK BL31 start\n");
+	/* Init delay function */
+	generic_delay_timer_init();
+	/* Initialize module initcall */
+	mtk_init_one_level(MTK_INIT_LVL_EARLY_PLAT);
+}
+
+void bl31_plat_arch_setup(void)
+{
+	const mmap_region_t bl_regions[] = {
+		MAP_BL_RO,
+		MAP_BL_RW,
+#if USE_COHERENT_MEM
+		MAP_BL_COHERENT_RAM,
+#endif
+		{0},
+	};
+
+	mtk_xlat_init(bl_regions);
+	/* Initialize module initcall */
+	mtk_init_one_level(MTK_INIT_LVL_ARCH);
+}
+
+/*****************************************************************************
+ * Perform any BL31 platform setup common to ARM standard platforms
+ ******************************************************************************/
+
+void bl31_platform_setup(void)
+{
+	mtk_init_one_level(MTK_INIT_LVL_PLAT_SETUP_0);
+	mtk_init_one_level(MTK_INIT_LVL_PLAT_SETUP_1);
+}
+
+/*******************************************************************************
+ * Operations before cold CPU leave BL31.
+ * Switch console to runtime state.
+ ******************************************************************************/
+void bl31_plat_runtime_setup(void)
+{
+	mtk_init_one_level(MTK_INIT_LVL_PLAT_RUNTIME);
+	console_switch_state(CONSOLE_FLAG_RUNTIME);
+}
+
+unsigned int plat_get_syscnt_freq2(void)
+{
+	return SYS_COUNTER_FREQ_IN_HZ;
+}
diff --git a/plat/mediatek/common/mtk_plat_common.c b/plat/mediatek/common/mtk_plat_common.c
new file mode 100644
index 0000000..76f74a9
--- /dev/null
+++ b/plat/mediatek/common/mtk_plat_common.c
@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/arm/cci.h>
+#include <drivers/console.h>
+#include <lib/mmio.h>
+#include <lib/smccc.h>
+#include <lib/xlat_tables/xlat_tables.h>
+#include <plat/common/platform.h>
+#include <services/arm_arch_svc.h>
+
+#include <mtk_plat_common.h>
+#include <mtk_sip_svc.h>
+#include <plat_private.h>
+
+void clean_top_32b_of_param(uint32_t smc_fid,
+				u_register_t *px1,
+				u_register_t *px2,
+				u_register_t *px3,
+				u_register_t *px4)
+{
+	/* if parameters from SMC32. Clean top 32 bits */
+	if (GET_SMC_CC(smc_fid) == SMC_64) {
+		*px1 = *px1 & SMC32_PARAM_MASK;
+		*px2 = *px2 & SMC32_PARAM_MASK;
+		*px3 = *px3 & SMC32_PARAM_MASK;
+		*px4 = *px4 & SMC32_PARAM_MASK;
+	}
+}
+
+/*****************************************************************************
+ * plat_is_smccc_feature_available() - This function checks whether SMCCC
+ *                                     feature is availabile for platform.
+ * @fid: SMCCC function id
+ *
+ * Return SMC_OK if SMCCC feature is available and SMC_ARCH_CALL_NOT_SUPPORTED
+ * otherwise.
+ *****************************************************************************/
+int32_t plat_is_smccc_feature_available(u_register_t fid)
+{
+	switch (fid) {
+	case SMCCC_ARCH_SOC_ID:
+		return SMC_ARCH_CALL_SUCCESS;
+	default:
+		return SMC_ARCH_CALL_NOT_SUPPORTED;
+	}
+}
+
+int32_t plat_get_soc_version(void)
+{
+	uint32_t manfid = SOC_ID_SET_JEP_106(JEDEC_MTK_BKID, JEDEC_MTK_MFID);
+
+	return (int32_t)(manfid | (SOC_CHIP_ID & SOC_ID_IMPL_DEF_MASK));
+}
+
+int32_t plat_get_soc_revision(void)
+{
+	return 0;
+}
diff --git a/plat/mediatek/common/mtk_plat_common.h b/plat/mediatek/common/mtk_plat_common.h
new file mode 100644
index 0000000..4c14b9d
--- /dev/null
+++ b/plat/mediatek/common/mtk_plat_common.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef MTK_PLAT_COMMON_H
+#define MTK_PLAT_COMMON_H
+
+#include <stdint.h>
+
+#include <common/bl_common.h>
+#include <common/param_header.h>
+
+/*******************************************************************************
+ * Function and variable prototypes
+ ******************************************************************************/
+#define SMC32_PARAM_MASK		(0xFFFFFFFF)
+
+#define JEDEC_MTK_BKID U(4)
+#define JEDEC_MTK_MFID U(0x26)
+
+struct mtk_bl31_params {
+       param_header_t h;
+       image_info_t *bl31_image_info;
+       entry_point_info_t *bl32_ep_info;
+       image_info_t *bl32_image_info;
+       entry_point_info_t *bl33_ep_info;
+       image_info_t *bl33_image_info;
+};
+
+/* Declarations for mtk_plat_common.c */
+uint32_t plat_get_spsr_for_bl32_entry(void);
+uint32_t plat_get_spsr_for_bl33_entry(void);
+void clean_top_32b_of_param(uint32_t smc_fid, u_register_t *x1,
+				u_register_t *x2,
+				u_register_t *x3,
+				u_register_t *x4);
+void bl31_prepare_kernel_entry(uint64_t k32_64);
+void enable_ns_access_to_cpuectlr(void);
+void boot_to_kernel(uint64_t x1, uint64_t x2, uint64_t x3, uint64_t x4);
+uint64_t get_kernel_info_pc(void);
+uint64_t get_kernel_info_r0(void);
+uint64_t get_kernel_info_r1(void);
+uint64_t get_kernel_info_r2(void);
+
+extern struct atf_arg_t gteearg;
+#endif /* MTK_PLAT_COMMON_H */
diff --git a/plat/mediatek/common/mtk_sip_svc.c b/plat/mediatek/common/mtk_sip_svc.c
new file mode 100644
index 0000000..dab0d45
--- /dev/null
+++ b/plat/mediatek/common/mtk_sip_svc.c
@@ -0,0 +1,129 @@
+/*
+ * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <drivers/console.h>
+#include <lib/mmio.h>
+#include <tools_share/uuid.h>
+
+#include <mtk_plat_common.h>
+#include <mtk_sip_svc.h>
+#include <plat_sip_calls.h>
+
+/* Mediatek SiP Service UUID */
+DEFINE_SVC_UUID2(mtk_sip_svc_uid,
+	0xa42b58f7, 0x6242, 0x7d4d, 0x80, 0xe5,
+	0x8f, 0x95, 0x05, 0x00, 0x0f, 0x3d);
+
+#pragma weak mediatek_plat_sip_handler
+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)
+{
+	ERROR("%s: unhandled SMC (0x%x)\n", __func__, smc_fid);
+	SMC_RET1(handle, SMC_UNK);
+}
+
+/*
+ * This function handles Mediatek defined SiP Calls */
+uintptr_t mediatek_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)
+{
+	uint32_t ns;
+
+	/* if parameter is sent from SMC32. Clean top 32 bits */
+	clean_top_32b_of_param(smc_fid, &x1, &x2, &x3, &x4);
+
+	/* Determine which security state this SMC originated from */
+	ns = is_caller_non_secure(flags);
+	if (!ns) {
+		/* SiP SMC service secure world's call */
+		;
+	} else {
+		/* SiP SMC service normal world's call */
+		switch (smc_fid) {
+#if MTK_SIP_SET_AUTHORIZED_SECURE_REG_ENABLE
+		case MTK_SIP_SET_AUTHORIZED_SECURE_REG: {
+			/* only use ret here */
+			uint64_t ret;
+
+			ret = mt_sip_set_authorized_sreg((uint32_t)x1,
+				(uint32_t)x2);
+			SMC_RET1(handle, ret);
+		}
+#endif
+#if MTK_SIP_KERNEL_BOOT_ENABLE
+		case MTK_SIP_KERNEL_BOOT_AARCH32:
+			boot_to_kernel(x1, x2, x3, x4);
+			SMC_RET0(handle);
+#endif
+		default:
+			/* Do nothing in default case */
+			break;
+		}
+	}
+
+	return mediatek_plat_sip_handler(smc_fid, x1, x2, x3, x4,
+					cookie, handle, flags);
+
+}
+
+/*
+ * This function is responsible for handling all SiP calls from the NS world
+ */
+uintptr_t sip_smc_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) {
+	case SIP_SVC_CALL_COUNT:
+		/* Return the number of Mediatek SiP Service Calls. */
+		SMC_RET1(handle,
+			 MTK_COMMON_SIP_NUM_CALLS + MTK_PLAT_SIP_NUM_CALLS);
+
+	case SIP_SVC_UID:
+		/* Return UID to the caller */
+		SMC_UUID_RET(handle, mtk_sip_svc_uid);
+
+	case SIP_SVC_VERSION:
+		/* Return the version of current implementation */
+		SMC_RET2(handle, MTK_SIP_SVC_VERSION_MAJOR,
+			MTK_SIP_SVC_VERSION_MINOR);
+
+	default:
+		return mediatek_sip_handler(smc_fid, x1, x2, x3, x4,
+			cookie, handle, flags);
+	}
+}
+
+/* Define a runtime service descriptor for fast SMC calls */
+DECLARE_RT_SVC(
+	mediatek_sip_svc,
+	OEN_SIP_START,
+	OEN_SIP_END,
+	SMC_TYPE_FAST,
+	NULL,
+	sip_smc_handler
+);
diff --git a/plat/mediatek/common/mtk_smc_handlers.c b/plat/mediatek/common/mtk_smc_handlers.c
new file mode 100644
index 0000000..5a3ad1f
--- /dev/null
+++ b/plat/mediatek/common/mtk_smc_handlers.c
@@ -0,0 +1,259 @@
+/*
+ * Copyright (c) 2022-2023, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#if MTK_SIP_KERNEL_BOOT_ENABLE
+#include <cold_boot.h>
+#endif
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <lib/mtk_init/mtk_init.h>
+#include <mtk_sip_svc.h>
+
+#define SMC_HANDLER_DEBUG(...) VERBOSE(__VA_ARGS__)
+#define SMC_HANDLER_DEBUG_NOT_IMP_MSG "%s[0x%x] smc handler not implemented\n"
+#define SMC_HANDLER_DEBUG_START_MSG "%s[0x%x] smc handler start, smc desc. index:%d\n"
+#define SMC_HANDLER_DEBUG_END_MSG "%s[0x%x] smc handler end\n"
+
+/*
+ * These macros below are used to identify SIP calls from Kernel,
+ * Hypervisor, or 2ndBootloader
+ */
+#define SIP_FID_ORI_MASK	(0xc000)
+#define SIP_FID_ORI_SHIFT	(14)
+#define SIP_FID_KERNEL		(0x0)
+#define SIP_FID_KERNEL_VIA_GZ	(0x1)
+#define SIP_FID_GZ		(0x2)
+
+#define GET_SMC_ORI(_fid)	(((_fid) & SIP_FID_ORI_MASK) >> SIP_FID_ORI_SHIFT)
+#define GET_SMC_ORI_NUM(_fid)	((_fid) & ~(SIP_FID_ORI_MASK))
+
+#define is_from_nsel2(_ori)	(_ori == SIP_FID_GZ)
+#define is_from_bl33(_ori) \
+	((_ori != SIP_FID_GZ) && (is_el1_2nd_bootloader() == 1))
+#define is_from_nsel1(_ori) \
+	(((_ori == SIP_FID_KERNEL) || \
+	 (_ori == SIP_FID_KERNEL_VIA_GZ)) && \
+	 (is_el1_2nd_bootloader() == 0))
+
+#define is_smc_forbidden(_ori) (_ori == SIP_FID_KERNEL_VIA_GZ)
+
+#define MASK_32_BIT (0xffffffffU)
+#define SMC_ID_EXPAND_AS_SMC_OPERATION(_smc_id, _smc_num) \
+	case _smc_id##_AARCH32: \
+	{ \
+		x1 = x1 & MASK_32_BIT; \
+		x2 = x2 & MASK_32_BIT; \
+		x3 = x3 & MASK_32_BIT; \
+		x4 = x4 & MASK_32_BIT; \
+	} \
+	/* fallthrough */ \
+	case _smc_id##_AARCH64: \
+	{ \
+		if (_smc_id##_descriptor_index < 0) { \
+			SMC_HANDLER_DEBUG(SMC_HANDLER_DEBUG_NOT_IMP_MSG, #_smc_id, smc_id); \
+			break; \
+		} \
+		if (_smc_id##_descriptor_index >= smc_id_descriptor_max) { \
+			SMC_HANDLER_DEBUG("smc descriptor index[%d] exceed max[%d]\n", \
+					  _smc_id##_descriptor_index, smc_id_descriptor_max); \
+			break; \
+		} \
+		SMC_HANDLER_DEBUG(SMC_HANDLER_DEBUG_START_MSG, #_smc_id, smc_id, \
+				  _smc_id##_descriptor_index); \
+		ret  = smc_handler_pool[_smc_id##_descriptor_index].smc_handler(x1,\
+				x2, x3, x4, handle, &smc_ret); \
+		SMC_HANDLER_DEBUG(SMC_HANDLER_DEBUG_END_MSG, #_smc_id, smc_id); \
+		break; \
+	}
+
+#define SMC_ID_EXPAND_AS_DESCRIPTOR_INDEX(_smc_id, _smc_num) \
+	short _smc_id##_descriptor_index __section(".mtk_plat_ro") = -1;
+
+MTK_SIP_SMC_FROM_BL33_TABLE(SMC_ID_EXPAND_AS_DESCRIPTOR_INDEX);
+MTK_SIP_SMC_FROM_NS_EL1_TABLE(SMC_ID_EXPAND_AS_DESCRIPTOR_INDEX);
+MTK_SIP_SMC_FROM_S_EL1_TABLE(SMC_ID_EXPAND_AS_DESCRIPTOR_INDEX);
+
+IMPORT_SYM(uintptr_t, __MTK_SMC_POOL_START__, MTK_SMC_POOL_START);
+IMPORT_SYM(uintptr_t, __MTK_SMC_POOL_END_UNALIGNED__, MTK_SMC_POOL_END_UNALIGNED);
+
+static const struct smc_descriptor *smc_handler_pool;
+static short smc_id_descriptor_max;
+
+#if !MTK_SIP_KERNEL_BOOT_ENABLE
+/*
+ * If there is no SMC request needs to be served in 2nd bootloader,
+ * disable the service path inherently.
+ */
+bool is_el1_2nd_bootloader(void)
+{
+	return false;
+}
+#endif
+
+static void print_smc_descriptor(const struct smc_descriptor pool[])
+{
+	const struct smc_descriptor *p_smc_desc;
+
+	INFO("print smc descriptor pool\n");
+	for (p_smc_desc = &pool[0];
+	     (char *)p_smc_desc < (char *)MTK_SMC_POOL_END_UNALIGNED;
+	     p_smc_desc++) {
+		INFO("descriptor name:%s\n", p_smc_desc->smc_name);
+		INFO("descriptor index:%d\n", *p_smc_desc->smc_descriptor_index);
+		INFO("smc id 32:0x%x, smc id 64:0x%x\n",
+		     p_smc_desc->smc_id_aarch32, p_smc_desc->smc_id_aarch64);
+	}
+}
+
+static int mtk_smc_handler_init(void)
+{
+	const struct smc_descriptor *iter;
+	short index_cnt;
+	int ret = 0;
+
+	smc_handler_pool = (const struct smc_descriptor *)MTK_SMC_POOL_START;
+	/* Designate descriptor index point to smc_handler_pool */
+	for (index_cnt = 0, iter = &smc_handler_pool[0];
+	     (char *)iter < (char *)MTK_SMC_POOL_END_UNALIGNED;
+	     iter++, index_cnt++) {
+		if (index_cnt < 0) {
+			SMC_HANDLER_DEBUG("smc handler pool index overflow!\n");
+			ret = -EPERM;
+			assert(0);
+			break;
+		}
+		*(iter->smc_descriptor_index) = index_cnt;
+	}
+	smc_id_descriptor_max = index_cnt;
+	print_smc_descriptor(smc_handler_pool);
+	return ret;
+}
+MTK_EARLY_PLAT_INIT(mtk_smc_handler_init);
+
+/* This function handles Mediatek defined SiP Calls from Secure world */
+static u_register_t mtk_smc_handler_sel1(uint32_t smc_id,
+					 u_register_t x1,
+					 u_register_t x2,
+					 u_register_t x3,
+					 u_register_t x4,
+					 void *cookie,
+					 void *handle,
+					 u_register_t flags)
+{
+	u_register_t ret = MTK_SIP_E_SUCCESS;
+	struct smccc_res smc_ret = {0};
+
+	switch (smc_id) {
+		MTK_SIP_SMC_FROM_S_EL1_TABLE(SMC_ID_EXPAND_AS_SMC_OPERATION);
+	default:
+		INFO("SEL1 SMC ID:0x%x not support\n", smc_id);
+		ret = SMC_UNK;
+	}
+	SMC_RET4(handle, ret, smc_ret.a1, smc_ret.a2, smc_ret.a3);
+}
+
+/* This function handles Mediatek defined SiP Calls from Bootloader */
+static uintptr_t mtk_smc_handler_bl33(uint32_t smc_id,
+				      u_register_t x1,
+				      u_register_t x2,
+				      u_register_t x3,
+				      u_register_t x4,
+				      void *cookie,
+				      void *handle,
+				      u_register_t flags)
+{
+	uintptr_t ret = MTK_SIP_E_SUCCESS;
+	struct smccc_res smc_ret = {0};
+
+	switch (smc_id) {
+		MTK_SIP_SMC_FROM_BL33_TABLE(SMC_ID_EXPAND_AS_SMC_OPERATION);
+	default:
+		INFO("BL33 SMC ID:0x%x not supported\n", smc_id);
+		ret = SMC_UNK;
+		break;
+	}
+	SMC_RET4(handle, ret, smc_ret.a1, smc_ret.a2, smc_ret.a3);
+}
+
+/* This function handles Mediatek defined SiP Calls from Kernel */
+static uintptr_t mtk_smc_handler_nsel1(uint32_t smc_id,
+				       u_register_t x1,
+				       u_register_t x2,
+				       u_register_t x3,
+				       u_register_t x4,
+				       void *cookie,
+				       void *handle,
+				       u_register_t flags)
+{
+	uintptr_t ret = MTK_SIP_E_SUCCESS;
+	struct smccc_res smc_ret = {0};
+
+	switch (smc_id) {
+		MTK_SIP_SMC_FROM_NS_EL1_TABLE(SMC_ID_EXPAND_AS_SMC_OPERATION);
+	default:
+		INFO("NSEL1 SMC ID:0x%x not supported\n", smc_id);
+		ret = SMC_UNK;
+		break;
+	}
+	SMC_RET4(handle, ret, smc_ret.a1, smc_ret.a2, smc_ret.a3);
+}
+
+static uintptr_t mtk_smc_handler(uint32_t smc_id,
+				 u_register_t x1,
+				 u_register_t x2,
+				 u_register_t x3,
+				 u_register_t x4,
+				 void *cookie,
+				 void *handle,
+				 u_register_t flags)
+{
+	uintptr_t ret = SMC_UNK;
+	uint32_t ns;
+	uint32_t smc_ori;
+	uint32_t smc_num;
+
+	/* Get SMC Originator bit 14.15 */
+	smc_ori = GET_SMC_ORI(smc_id);
+	/* Get SMC Number. Clean bit 14.15 */
+	smc_num = GET_SMC_ORI_NUM(smc_id);
+
+	/* Determine which security state this SMC originated from */
+	ns = is_caller_non_secure(flags);
+
+	if (ns && is_smc_forbidden(smc_ori)) {
+		ERROR("%s: Forbidden SMC call (0x%x)\n", __func__, smc_id);
+		SMC_RET1(handle, ret);
+	}
+
+	if (!ns) {
+		/* SiP SMC service secure world's call */
+		return mtk_smc_handler_sel1(smc_num, x1, x2, x3, x4,
+					    cookie, handle, flags);
+	}
+	if (is_from_bl33(smc_ori)) {
+		/* SiP SMC service secure bootloader's call */
+		return mtk_smc_handler_bl33(smc_num, x1, x2, x3, x4,
+					    cookie, handle, flags);
+	} else if (is_from_nsel1(smc_ori)) {
+		/* SiP SMC service kernel's call */
+		return mtk_smc_handler_nsel1(smc_num, x1, x2, x3, x4,
+					     cookie, handle, flags);
+	}
+	INFO("SMC ID:0x%x not supported\n", smc_id);
+	SMC_RET1(handle, ret);
+}
+
+/* Define a runtime service descriptor for fast SMC calls */
+DECLARE_RT_SVC(
+	mtk_smc_handler,
+	OEN_SIP_START,
+	OEN_SIP_END,
+	SMC_TYPE_FAST,
+	NULL,
+	mtk_smc_handler
+);
diff --git a/plat/mediatek/common/params_setup.c b/plat/mediatek/common/params_setup.c
new file mode 100644
index 0000000..a9df13e
--- /dev/null
+++ b/plat/mediatek/common/params_setup.c
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2019, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/bl_aux_params/bl_aux_params.h>
+#include <common/debug.h>
+#include <plat_params.h>
+#include <string.h>
+
+static struct bl_aux_gpio_info rst_gpio;
+
+struct bl_aux_gpio_info *plat_get_mtk_gpio_reset(void)
+{
+	return &rst_gpio;
+}
+
+static bool mtk_aux_param_handler(struct bl_aux_param_header *param)
+{
+	/* Store platform parameters for later processing if needed. */
+	switch (param->type) {
+	case BL_AUX_PARAM_MTK_RESET_GPIO:
+		rst_gpio = ((struct bl_aux_param_gpio *)param)->gpio;
+		return true;
+	}
+
+	return false;
+}
+
+void params_early_setup(u_register_t plat_param_from_bl2)
+{
+	bl_aux_params_parse(plat_param_from_bl2, mtk_aux_param_handler);
+}
+
diff --git a/plat/mediatek/common/plat_params.h b/plat/mediatek/common/plat_params.h
new file mode 100644
index 0000000..828c3dc
--- /dev/null
+++ b/plat/mediatek/common/plat_params.h
@@ -0,0 +1,17 @@
+/*
+ * Copyright (c) 2019, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_PARAMS_H
+#define PLAT_PARAMS_H
+
+#include <stdint.h>
+
+#include <export/plat/mediatek/common/plat_params_exp.h>
+
+struct bl_aux_gpio_info *plat_get_mtk_gpio_reset(void);
+void params_early_setup(u_register_t plat_param_from_bl2);
+
+#endif
diff --git a/plat/mediatek/common/rules.mk b/plat/mediatek/common/rules.mk
new file mode 100644
index 0000000..6acc731
--- /dev/null
+++ b/plat/mediatek/common/rules.mk
@@ -0,0 +1,15 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := mtk_common
+
+LOCAL_SRCS-y := ${LOCAL_DIR}/mtk_bl31_setup.c
+LOCAL_SRCS-y += ${LOCAL_DIR}/mtk_smc_handlers.c
+LOCAL_SRCS-$(MTK_SIP_KERNEL_BOOT_ENABLE) += ${LOCAL_DIR}/cold_boot.c
+
+$(eval $(call MAKE_LOCALS,$(LOCAL_SRCS-y),$(MTK_BL)))