plat/mediatek/common/cold_boot.c - filogic/atf - Gitiles

 /*
  * 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);
	/*
	* 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);