diff --git a/plat/imx/common/imx_sip_handler.c b/plat/imx/common/imx_sip_handler.c index 62048b6..f9f5577 100644 --- a/plat/imx/common/imx_sip_handler.c +++ b/plat/imx/common/imx_sip_handler.c @@ -4,6 +4,7 @@ * SPDX-License-Identifier: BSD-3-Clause */ +#include #include #include #include @@ -12,6 +13,7 @@ #include #include #include +#include #include #if defined(PLAT_imx8qm) || defined(PLAT_imx8qx) @@ -185,3 +187,37 @@ return ret; } + +int imx_kernel_entry_handler(uint32_t smc_fid, + u_register_t x1, + u_register_t x2, + u_register_t x3, + u_register_t x4) +{ + static entry_point_info_t bl33_image_ep_info; + entry_point_info_t *next_image_info; + unsigned int mode; + + if (x1 < (PLAT_NS_IMAGE_OFFSET & 0xF0000000)) + return SMC_UNK; + + mode = MODE32_svc; + + next_image_info = &bl33_image_ep_info; + + next_image_info->pc = x1; + + 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->args.arg0 = 0; + next_image_info->args.arg1 = 0; + next_image_info->args.arg2 = x3; + + SET_SECURITY_STATE(next_image_info->h.attr, NON_SECURE); + + cm_init_my_context(next_image_info); + cm_prepare_el3_exit(NON_SECURE); + + return 0; +} diff --git a/plat/imx/common/imx_sip_svc.c b/plat/imx/common/imx_sip_svc.c index 4893b9f..20e1479 100644 --- a/plat/imx/common/imx_sip_svc.c +++ b/plat/imx/common/imx_sip_svc.c @@ -26,6 +26,9 @@ u_register_t flags) { switch (smc_fid) { + case IMX_SIP_AARCH32: + SMC_RET1(handle, imx_kernel_entry_handler(smc_fid, x1, x2, x3, x4)); + break; #if defined(PLAT_imx8mq) case IMX_SIP_GET_SOC_INFO: SMC_RET1(handle, imx_soc_info_handler(smc_fid, x1, x2, x3)); diff --git a/plat/imx/common/include/imx_sip_svc.h b/plat/imx/common/include/imx_sip_svc.h index 5898f7a..0a2d750 100644 --- a/plat/imx/common/include/imx_sip_svc.h +++ b/plat/imx/common/include/imx_sip_svc.h @@ -28,6 +28,11 @@ #define IMX_SIP_MISC_SET_TEMP 0xC200000C +#define IMX_SIP_AARCH32 0xC20000FD + +int imx_kernel_entry_handler(uint32_t smc_fid, u_register_t x1, + u_register_t x2, u_register_t x3, + u_register_t x4); #if defined(PLAT_imx8mq) int imx_soc_info_handler(uint32_t smc_fid, u_register_t x1, u_register_t x2, u_register_t x3);