diff --git a/plat/mediatek/common/cold_boot.c b/plat/mediatek/common/cold_boot.c
new file mode 100644
index 0000000..ff585fe
--- /dev/null
+++ b/plat/mediatek/common/cold_boot.c
@@ -0,0 +1,336 @@
+/*
+ * Copyright (c) 2022, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <inttypes.h>
+#include <stdint.h>
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <lib/el3_runtime/context_mgmt.h>
+
+/* Vendors headers */
+#include <cold_boot.h>
+#include <lib/mtk_init/mtk_init.h>
+#include <mtk_sip_svc.h>
+
+static struct kernel_info k_info;
+static entry_point_info_t bl32_image_ep_info;
+static entry_point_info_t bl33_image_ep_info;
+static bool el1_is_2nd_bootloader = true;
+static struct atf_arg_t atfarg;
+
+static int init_mtk_bl32_arg(void)
+{
+	struct mtk_bl_param_t *p_mtk_bl_param;
+	struct atf_arg_t *p_atfarg;
+
+	p_mtk_bl_param = (struct mtk_bl_param_t *) get_mtk_bl31_fw_config(BOOT_ARG_FROM_BL2);
+	if (p_mtk_bl_param == NULL) {
+		ERROR("p_mtk_bl_param is NULL!\n");
+		return -1;
+	}
+	p_atfarg = (struct atf_arg_t *)p_mtk_bl_param->atf_arg_addr;
+	if (p_atfarg == NULL) {
+		ERROR("bl32 argument is NULL!\n");
+		return -1;
+	}
+	memcpy((void *)&atfarg, (void *)p_atfarg, sizeof(struct atf_arg_t));
+	return 0;
+}
+MTK_EARLY_PLAT_INIT(init_mtk_bl32_arg);
+
+static void save_kernel_info(uint64_t pc, uint64_t r0, uint64_t r1, uint64_t k32_64)
+{
+	k_info.k32_64 = k32_64;
+	k_info.pc = pc;
+
+	if (k32_64 == LINUX_KERNEL_32) {
+		/* for 32 bits kernel */
+		k_info.r0 = 0;
+		/* machtype */
+		k_info.r1 = r0;
+		/* tags */
+		k_info.r2 = r1;
+	} else {
+		/* for 64 bits kernel */
+		k_info.r0 = r0;
+		k_info.r1 = r1;
+	}
+}
+
+static uint32_t plat_get_spsr_for_bl32_64_entry(void)
+{
+	return SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+}
+
+#if MTK_BL33_IS_64BIT
+static uint32_t plat_get_spsr_for_bl33_entry(void)
+{
+	uint32_t spsr;
+	uint32_t mode;
+
+	mode = MODE_EL1;
+	spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+	return spsr;
+}
+#else
+static uint32_t plat_get_spsr_for_bl33_entry(void)
+{
+	unsigned int mode;
+	uint32_t spsr;
+	unsigned int ee;
+	unsigned long daif;
+
+	INFO("Secondary bootloader is AArch32\n");
+	mode = MODE32_svc;
+	ee = 0;
+	/*
+	 * TODO: Choose async. exception bits if HYP mode is not
+	 * implemented according to the values of SCR.{AW, FW} bits
+	 */
+	daif = DAIF_ABT_BIT | DAIF_IRQ_BIT | DAIF_FIQ_BIT;
+
+	spsr = SPSR_MODE32(mode, 0, ee, daif);
+	return spsr;
+}
+#endif
+
+static void populate_bl32_image_ep(entry_point_info_t *bl32_ep_instance,
+		struct mtk_bl_param_t *p_mtk_bl_param)
+{
+	entry_point_info_t *populated_ep_bl32 = bl32_ep_instance;
+
+	if (p_mtk_bl_param == NULL) {
+		ERROR("p_mtk_bl_param is NULL!\n");
+		panic();
+	}
+	SET_SECURITY_STATE(bl32_ep_instance->h.attr, SECURE);
+	SET_PARAM_HEAD(populated_ep_bl32,
+		       PARAM_EP,
+		       VERSION_1,
+		       populated_ep_bl32->h.attr);
+	populated_ep_bl32->pc = atfarg.tee_entry;
+	populated_ep_bl32->spsr = plat_get_spsr_for_bl32_64_entry();
+}
+
+static void populate_bl33_image_ep(entry_point_info_t *bl33_ep_instance,
+		struct mtk_bl_param_t *p_mtk_bl_param)
+{
+	entry_point_info_t *populated_ep_bl33 = bl33_ep_instance;
+
+	if (p_mtk_bl_param == NULL) {
+		ERROR("p_mtk_bl_param is NULL!\n");
+		panic();
+	}
+	SET_SECURITY_STATE(bl33_ep_instance->h.attr, NON_SECURE);
+	SET_PARAM_HEAD(populated_ep_bl33,
+		       PARAM_EP,
+		       VERSION_1,
+		       populated_ep_bl33->h.attr);
+	populated_ep_bl33->pc = p_mtk_bl_param->bl33_start_addr;
+	/* standardize 2nd bootloader input argument */
+	populated_ep_bl33->args.arg0 = p_mtk_bl_param->bootarg_loc;
+	/* compatible to old GZ version */
+	populated_ep_bl33->args.arg4 = p_mtk_bl_param->bootarg_loc;
+	populated_ep_bl33->args.arg5 = p_mtk_bl_param->bootarg_size;
+	populated_ep_bl33->spsr = plat_get_spsr_for_bl33_entry();
+}
+
+static int populate_bl_images_ep(struct mtk_bl_param_t *p_mtk_bl_param)
+{
+	/*
+	 * Tell BL31 where the non-trusted software image
+	 * is located and the entry state information
+	 */
+	populate_bl33_image_ep(&bl33_image_ep_info, p_mtk_bl_param);
+	populate_bl32_image_ep(&bl32_image_ep_info, p_mtk_bl_param);
+	return 0;
+}
+
+static int populate_bl_images_ep_init(void)
+{
+	return populate_bl_images_ep(get_mtk_bl31_fw_config(BOOT_ARG_FROM_BL2));
+}
+MTK_PLAT_SETUP_0_INIT(populate_bl_images_ep_init);
+
+static entry_point_info_t *bl31_plat_get_next_kernel64_ep_info(void)
+{
+	entry_point_info_t *next_image_info;
+	unsigned long el_status;
+	unsigned int mode;
+
+	el_status = 0;
+	mode = 0;
+
+	/* Kernel image is always non-secured */
+	next_image_info = &bl33_image_ep_info;
+
+	/* Figure out what mode we enter the non-secure world in */
+	el_status = read_id_aa64pfr0_el1() >> ID_AA64PFR0_EL2_SHIFT;
+	el_status &= ID_AA64PFR0_ELX_MASK;
+
+	INFO("Kernel_EL %d\n", el_status?2:1);
+	if (el_status) {
+		mode = MODE_EL2;
+	} else {
+		mode = MODE_EL1;
+	}
+	INFO("Kernel is 64Bit\n");
+	next_image_info->spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+	next_image_info->pc = k_info.pc;
+	next_image_info->args.arg0 = k_info.r0;
+	next_image_info->args.arg1 = k_info.r1;
+
+	INFO("pc=0x%lx, r0=0x%" PRIx64 ", r1=0x%" PRIx64 "\n",
+	     next_image_info->pc,
+	     next_image_info->args.arg0,
+	     next_image_info->args.arg1);
+
+	SET_SECURITY_STATE(next_image_info->h.attr, NON_SECURE);
+
+	/* None of the images on this platform can have 0x0 as the entrypoint */
+	if (next_image_info->pc) {
+		return next_image_info;
+	}
+
+	return NULL;
+}
+
+static entry_point_info_t *bl31_plat_get_next_kernel32_ep_info(void)
+{
+	entry_point_info_t *next_image_info;
+	unsigned int mode;
+
+	mode = 0;
+
+	/* Kernel image is always non-secured */
+	next_image_info = &bl33_image_ep_info;
+
+	/* Figure out what mode we enter the non-secure world in */
+	mode = MODE32_hyp;
+	/*
+	 * TODO: Consider the possibility of specifying the SPSR in
+	 * the FIP ToC and allowing the platform to have a say as
+	 * well.
+	 */
+
+	INFO("Kernel is 32Bit\n");
+	next_image_info->spsr = SPSR_MODE32(mode, SPSR_T_ARM, SPSR_E_LITTLE,
+					    (DAIF_FIQ_BIT | DAIF_IRQ_BIT | DAIF_ABT_BIT));
+	next_image_info->pc = k_info.pc;
+	next_image_info->args.arg0 = k_info.r0;
+	next_image_info->args.arg1 = k_info.r1;
+	next_image_info->args.arg2 = k_info.r2;
+
+	INFO("pc=0x%lx, r0=0x%" PRIx64 ", r1=0x%" PRIx64 ", r2=0x%" PRIx64 "\n",
+	     next_image_info->pc,
+	     next_image_info->args.arg0,
+	     next_image_info->args.arg1,
+	     next_image_info->args.arg2);
+
+	SET_SECURITY_STATE(next_image_info->h.attr, NON_SECURE);
+
+	/* None of the images on this platform can have 0x0 as the entrypoint */
+	if (next_image_info->pc) {
+		return next_image_info;
+	}
+
+	return NULL;
+}
+
+static void bl31_prepare_kernel_entry(uint64_t k32_64)
+{
+	entry_point_info_t *next_image_info = NULL;
+	uint32_t image_type;
+
+	/* Determine which image to execute next */
+	image_type = NON_SECURE; /* bl31_get_next_image_type(); */
+
+	/* Leave 2nd bootloader then jump to kernel */
+	el1_is_2nd_bootloader = false;
+
+	/* Program EL3 registers to enable entry into the next EL */
+	if (k32_64 == LINUX_KERNEL_32) {
+		next_image_info = bl31_plat_get_next_kernel32_ep_info();
+	} else {
+		next_image_info = bl31_plat_get_next_kernel64_ep_info();
+	}
+
+	assert(next_image_info);
+	assert(image_type == GET_SECURITY_STATE(next_image_info->h.attr));
+
+	INFO("BL31: Preparing for EL3 exit to %s world, Kernel\n",
+	     (image_type == SECURE) ? "secure" : "normal");
+	INFO("BL31: Next image address = 0x%" PRIx64 "\n",
+	     next_image_info->pc);
+	INFO("BL31: Next image spsr = 0x%x\n", next_image_info->spsr);
+	cm_init_my_context(next_image_info);
+	cm_prepare_el3_exit(image_type);
+}
+
+bool is_el1_2nd_bootloader(void)
+{
+	return el1_is_2nd_bootloader;
+}
+
+/*******************************************************************************
+ * 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_image_ep_info : &bl32_image_ep_info;
+
+	/* None of the images on this platform can have 0x0 as the entrypoint */
+	if (next_image_info->pc) {
+		return next_image_info;
+	}
+	return NULL;
+}
+
+u_register_t boot_to_kernel(u_register_t x1,
+			    u_register_t x2,
+			    u_register_t x3,
+			    u_register_t x4,
+			    void *handle,
+			    struct smccc_res *smccc_ret)
+{
+	static uint8_t kernel_boot_once_flag;
+
+	/* only support in booting flow */
+	if (kernel_boot_once_flag == 0) {
+		kernel_boot_once_flag = 1;
+
+		INFO("save kernel info\n");
+		save_kernel_info(x1, x2, x3, x4);
+		bl31_prepare_kernel_entry(x4);
+		INFO("el3_exit\n");
+		/*
+		 * FIXME: no better way so far to prevent from
+		 * SiP root handler wipe x0~x3 if not assign smccc_ret
+		 * return register
+		 */
+		smccc_ret->a1 = x3;
+
+		mtk_init_one_level(MTK_INIT_LVL_BL33_DEFER);
+
+#if MTK_CONSOLE_RUNTIME_DISABLE
+		INFO("Turn off BL31 console\n");
+		mtk_console_core_end();
+#endif
+
+		/* Re-assign as x0 register entering Linux kernel */
+		return x2;
+	}
+	return 0;
+}
+/* Register SiP SMC service */
+DECLARE_SMC_HANDLER(MTK_SIP_KERNEL_BOOT, boot_to_kernel);