From 7c3309c901f7336f4d459b99d180df0e6750c3d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arve=20Hj=C3=B8nnev=C3=A5g?= Date: Tue, 28 Nov 2017 14:05:30 -0800 Subject: trusty: Restore working trusty setup code and include trusty image in build MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Change-Id: I081901e7df22f78dd9c4fc4c6bfad2aceb870a2d Signed-off-by: Arve Hjønnevåg --- services/spd/trusty/trusty.c | 44 +++++++++++++++++++++++++++++++++---------- services/spd/trusty/trusty.mk | 2 ++ 2 files changed, 36 insertions(+), 10 deletions(-) (limited to 'services') diff --git a/services/spd/trusty/trusty.c b/services/spd/trusty/trusty.c index ecbcfaea..08767bb5 100644 --- a/services/spd/trusty/trusty.c +++ b/services/spd/trusty/trusty.c @@ -385,11 +385,23 @@ static const spd_pm_ops_t trusty_pm = { .svc_suspend_finish = trusty_cpu_suspend_finish_handler, }; +void plat_trusty_set_boot_args(aapcs64_params_t *args); + +#ifdef TSP_SEC_MEM_SIZE +#pragma weak plat_trusty_set_boot_args +void plat_trusty_set_boot_args(aapcs64_params_t *args) +{ + args->arg0 = TSP_SEC_MEM_SIZE; +} +#endif + static int32_t trusty_setup(void) { entry_point_info_t *ep_info; + uint32_t instr; uint32_t flags; int ret; + int aarch32 = 0; /* Get trusty's entry point info */ ep_info = bl31_plat_get_next_image_ep_info(SECURE); @@ -398,17 +410,29 @@ static int32_t trusty_setup(void) return -1; } - /* Trusty runs in AARCH64 mode */ - SET_PARAM_HEAD(ep_info, PARAM_EP, VERSION_1, SECURE | EP_ST_ENABLE); - ep_info->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS); + instr = *(uint32_t *)ep_info->pc; - /* - * arg0 = TZDRAM aperture available for BL32 - * arg1 = BL32 boot params - * arg2 = BL32 boot params length - */ - ep_info->args.arg1 = ep_info->args.arg2; - ep_info->args.arg2 = TRUSTY_PARAMS_LEN_BYTES; + if (instr >> 24 == 0xea) { + INFO("trusty: Found 32 bit image\n"); + aarch32 = 1; + } else if (instr >> 8 == 0xd53810 || instr >> 16 == 0x9400) { + INFO("trusty: Found 64 bit image\n"); + } else { + NOTICE("trusty: Found unknown image, 0x%x\n", instr); + } + + SET_PARAM_HEAD(ep_info, PARAM_EP, VERSION_1, SECURE | EP_ST_ENABLE); + if (!aarch32) + ep_info->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, + DISABLE_ALL_EXCEPTIONS); + else + ep_info->spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM, + SPSR_E_LITTLE, + DAIF_FIQ_BIT | + DAIF_IRQ_BIT | + DAIF_ABT_BIT); + memset(&ep_info->args, 0, sizeof(ep_info->args)); + plat_trusty_set_boot_args(&ep_info->args); /* register init handler */ bl31_register_bl32_init(trusty_init); diff --git a/services/spd/trusty/trusty.mk b/services/spd/trusty/trusty.mk index beca875e..920504c7 100644 --- a/services/spd/trusty/trusty.mk +++ b/services/spd/trusty/trusty.mk @@ -8,3 +8,5 @@ SPD_INCLUDES := SPD_SOURCES := services/spd/trusty/trusty.c \ services/spd/trusty/trusty_helpers.S + +NEED_BL32 := yes -- cgit v1.2.3