GK SDK 源码库: XMIPCLinuxV100R005C00SPC030 (kernel/tools/open_source excluded)

source/trusted-firmware/trusted-firmware-a-2.8/plat/arm/common/aarch64/arm_bl2_mem_params_desc.c

@@ -0,0 +1,227 @@
+/*
+ * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <platform_def.h>
+
+#include <common/bl_common.h>
+#include <common/desc_image_load.h>
+
+/*******************************************************************************
+ * Following descriptor provides BL image/ep information that gets used
+ * by BL2 to load the images and also subset of this information is
+ * passed to next BL image. The image loading sequence is managed by
+ * populating the images in required loading order. The image execution
+ * sequence is managed by populating the `next_handoff_image_id` with
+ * the next executable image id.
+ ******************************************************************************/
+static bl_mem_params_node_t bl2_mem_params_descs[] = {
+#ifdef SCP_BL2_BASE
+	/* Fill SCP_BL2 related information if it exists */
+	{
+		.image_id = SCP_BL2_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = SCP_BL2_BASE,
+		.image_info.image_max_size = PLAT_CSS_MAX_SCP_BL2_SIZE,
+
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+#endif /* SCP_BL2_BASE */
+
+#ifdef EL3_PAYLOAD_BASE
+	/* Fill EL3 payload related information (BL31 is EL3 payload)*/
+	{
+		.image_id = BL31_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = EL3_PAYLOAD_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t,
+			IMAGE_ATTRIB_PLAT_SETUP | IMAGE_ATTRIB_SKIP_LOADING),
+
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+
+#else /* EL3_PAYLOAD_BASE */
+
+	/* Fill BL31 related information */
+	{
+		.image_id = BL31_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = BL31_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
+#if DEBUG
+		.ep_info.args.arg3 = ARM_BL31_PLAT_PARAM_VAL,
+#endif
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
+		.image_info.image_base = BL31_BASE,
+		.image_info.image_max_size = BL31_LIMIT - BL31_BASE,
+
+# if defined(BL32_BASE)
+		.next_handoff_image_id = BL32_IMAGE_ID,
+# elif ENABLE_RME
+		.next_handoff_image_id = RMM_IMAGE_ID,
+# else
+		.next_handoff_image_id = BL33_IMAGE_ID,
+# endif
+	},
+	/* Fill HW_CONFIG related information */
+	{
+		.image_id = HW_CONFIG_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t,
+			NON_SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+	/* Fill SOC_FW_CONFIG related information */
+	{
+		.image_id = SOC_FW_CONFIG_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+
+# if ENABLE_RME
+	/* Fill RMM related information */
+	{
+		.image_id = RMM_IMAGE_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, EP_REALM | EXECUTABLE),
+		.ep_info.pc = RMM_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = RMM_BASE,
+		.image_info.image_max_size = RMM_LIMIT - RMM_BASE,
+		.next_handoff_image_id = BL33_IMAGE_ID,
+	},
+# endif
+
+# ifdef BL32_BASE
+	/* Fill BL32 related information */
+	{
+		.image_id = BL32_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | EXECUTABLE),
+		.ep_info.pc = BL32_BASE,
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+
+# if ENABLE_RME
+		.next_handoff_image_id = RMM_IMAGE_ID,
+# else
+		.next_handoff_image_id = BL33_IMAGE_ID,
+# endif
+	},
+
+	/*
+	 * Fill BL32 external 1 related information.
+	 * A typical use for extra1 image is with OP-TEE where it is the pager
+	 * image.
+	 */
+	{
+		.image_id = BL32_EXTRA1_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+
+	/*
+	 * Fill BL32 external 2 related information.
+	 * A typical use for extra2 image is with OP-TEE where it is the paged
+	 * image.
+	 */
+	{
+		.image_id = BL32_EXTRA2_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+#ifdef SPD_opteed
+		.image_info.image_base = ARM_OPTEE_PAGEABLE_LOAD_BASE,
+		.image_info.image_max_size = ARM_OPTEE_PAGEABLE_LOAD_SIZE,
+#endif
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+
+	/* Fill TOS_FW_CONFIG related information */
+	{
+		.image_id = TOS_FW_CONFIG_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+# endif /* BL32_BASE */
+
+	/* Fill BL33 related information */
+	{
+		.image_id = BL33_IMAGE_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
+# ifdef PRELOADED_BL33_BASE
+		.ep_info.pc = PRELOADED_BL33_BASE,
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+# else
+		.ep_info.pc = PLAT_ARM_NS_IMAGE_BASE,
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = PLAT_ARM_NS_IMAGE_BASE,
+		.image_info.image_max_size = ARM_DRAM1_BASE + ARM_DRAM1_SIZE
+			- PLAT_ARM_NS_IMAGE_BASE,
+# endif /* PRELOADED_BL33_BASE */
+
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+	/* Fill NT_FW_CONFIG related information */
+	{
+		.image_id = NT_FW_CONFIG_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t,
+			NON_SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	}
+#endif /* EL3_PAYLOAD_BASE */
+};
+
+REGISTER_BL_IMAGE_DESCS(bl2_mem_params_descs)

source/trusted-firmware/trusted-firmware-a-2.8/plat/arm/common/aarch64/arm_helpers.S

@@ -0,0 +1,136 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <asm_macros.S>
+#include <platform_def.h>
+
+	.weak	plat_arm_calc_core_pos
+	.weak	plat_my_core_pos
+	.globl	plat_crash_console_init
+	.globl	plat_crash_console_putc
+	.globl	plat_crash_console_flush
+	.globl	platform_mem_init
+
+
+	/* -----------------------------------------------------
+	 *  unsigned int plat_my_core_pos(void)
+	 *  This function uses the plat_arm_calc_core_pos()
+	 *  definition to get the index of the calling CPU.
+	 * -----------------------------------------------------
+	 */
+func plat_my_core_pos
+	mrs	x0, mpidr_el1
+	b	plat_arm_calc_core_pos
+endfunc plat_my_core_pos
+
+	/* -----------------------------------------------------
+	 *  unsigned int plat_arm_calc_core_pos(u_register_t mpidr)
+	 *  Helper function to calculate the core position.
+	 *  With this function: CorePos = (ClusterId * 4) +
+	 *  				  CoreId
+	 * -----------------------------------------------------
+	 */
+func plat_arm_calc_core_pos
+	and	x1, x0, #MPIDR_CPU_MASK
+	and	x0, x0, #MPIDR_CLUSTER_MASK
+	add	x0, x1, x0, LSR #6
+	ret
+endfunc plat_arm_calc_core_pos
+
+	/* ---------------------------------------------
+	 * int plat_crash_console_init(void)
+	 * Function to initialize the crash console
+	 * without a C Runtime to print crash report.
+	 * Clobber list : x0 - x4
+	 * ---------------------------------------------
+	 */
+func plat_crash_console_init
+	mov_imm	x0, PLAT_ARM_CRASH_UART_BASE
+	mov_imm	x1, PLAT_ARM_CRASH_UART_CLK_IN_HZ
+	mov_imm	x2, ARM_CONSOLE_BAUDRATE
+	b	console_pl011_core_init
+endfunc plat_crash_console_init
+
+	/* ---------------------------------------------
+	 * int plat_crash_console_putc(int c)
+	 * Function to print a character on the crash
+	 * console without a C Runtime.
+	 * Clobber list : x1, x2
+	 * ---------------------------------------------
+	 */
+func plat_crash_console_putc
+	mov_imm	x1, PLAT_ARM_CRASH_UART_BASE
+	b	console_pl011_core_putc
+endfunc plat_crash_console_putc
+
+	/* ---------------------------------------------
+	 * void plat_crash_console_flush()
+	 * Function to force a write of all buffered
+	 * data that hasn't been output.
+	 * Out : void.
+	 * Clobber list : r0
+	 * ---------------------------------------------
+	 */
+func plat_crash_console_flush
+	mov_imm	x0, PLAT_ARM_CRASH_UART_BASE
+	b	console_pl011_core_flush
+endfunc plat_crash_console_flush
+
+	/* ---------------------------------------------------------------------
+	 * We don't need to carry out any memory initialization on ARM
+	 * platforms. The Secure RAM is accessible straight away.
+	 * ---------------------------------------------------------------------
+	 */
+func platform_mem_init
+	ret
+endfunc platform_mem_init
+
+/*
+ * Need to use coherent stack when ARM Cryptocell is used to autheticate images
+ * since Cryptocell uses DMA to transfer data and it is not coherent with the
+ * AP CPU.
+ */
+#if ARM_CRYPTOCELL_INTEG
+#if defined(IMAGE_BL1) || defined(IMAGE_BL2)
+	.globl	plat_get_my_stack
+	.globl	plat_set_my_stack
+	.local	platform_coherent_stacks
+
+	/* -------------------------------------------------------
+	 * uintptr_t plat_get_my_stack ()
+	 *
+	 * For cold-boot BL images, only the primary CPU needs a
+	 * stack. This function returns the stack pointer for a
+	 * stack allocated in coherent memory.
+	 * -------------------------------------------------------
+	 */
+func plat_get_my_stack
+	get_up_stack platform_coherent_stacks, PLATFORM_STACK_SIZE
+	ret
+endfunc plat_get_my_stack
+
+	/* -------------------------------------------------------
+	 * void plat_set_my_stack ()
+	 *
+	 * For cold-boot BL images, only the primary CPU needs a
+	 * stack. This function sets the stack pointer to a stack
+	 * allocated in coherent memory.
+	 * -------------------------------------------------------
+	 */
+func plat_set_my_stack
+	get_up_stack platform_coherent_stacks, PLATFORM_STACK_SIZE
+	mov sp, x0
+	ret
+endfunc plat_set_my_stack
+
+	/* ----------------------------------------------------
+	 * Single cpu stack in coherent memory.
+	 * ----------------------------------------------------
+	 */
+declare_stack platform_coherent_stacks, tzfw_coherent_mem, \
+		PLATFORM_STACK_SIZE, 1, CACHE_WRITEBACK_GRANULE
+
+#endif	/* defined(IMAGE_BL1) || defined(IMAGE_BL2) */
+#endif	/* ARM_CRYPTOCELL_INTEG */

source/trusted-firmware/trusted-firmware-a-2.8/plat/arm/common/aarch64/arm_pauth.c

@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2019, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch_helpers.h>
+#include <cdefs.h>
+#include <stdint.h>
+
+/*
+ * This is only a toy implementation to generate a seemingly random
+ * 128-bit key from sp, x30 and cntpct_el0 values.
+ * A production system must re-implement this function to generate
+ * keys from a reliable randomness source.
+ */
+uint128_t plat_init_apkey(void)
+{
+	uint64_t return_addr = (uint64_t)__builtin_return_address(0U);
+	uint64_t frame_addr = (uint64_t)__builtin_frame_address(0U);
+	uint64_t cntpct = read_cntpct_el0();
+
+	/* Generate 128-bit key */
+	uint64_t key_lo = (return_addr << 13) ^ frame_addr ^ cntpct;
+	uint64_t key_hi = (frame_addr << 15) ^ return_addr ^ cntpct;
+
+	return ((uint128_t)(key_hi) << 64) | key_lo;
+}

source/trusted-firmware/trusted-firmware-a-2.8/plat/arm/common/aarch64/arm_sdei.c

@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/* SDEI configuration for ARM platforms */
+
+#include <bl31/ehf.h>
+#include <common/debug.h>
+#include <services/sdei.h>
+
+#if SDEI_IN_FCONF
+#include <plat/arm/common/fconf_sdei_getter.h>
+#endif
+#include <plat/common/platform.h>
+#include <platform_def.h>
+
+
+#if SDEI_IN_FCONF
+/* Private event mappings */
+static sdei_ev_map_t arm_sdei_private[PLAT_SDEI_DP_EVENT_MAX_CNT + 1] = { 0 };
+
+/* Shared event mappings */
+static sdei_ev_map_t arm_sdei_shared[PLAT_SDEI_DS_EVENT_MAX_CNT] = { 0 };
+
+void plat_sdei_setup(void)
+{
+	uint32_t i;
+
+	arm_sdei_private[0] = (sdei_ev_map_t)SDEI_DEFINE_EVENT_0(ARM_SDEI_SGI);
+
+	for (i = 0; i < FCONF_GET_PROPERTY(sdei, dyn_config, private_ev_cnt); i++) {
+		arm_sdei_private[i + 1] = (sdei_ev_map_t)SDEI_PRIVATE_EVENT(
+			FCONF_GET_PROPERTY(sdei, dyn_config, private_ev_nums[i]),
+			FCONF_GET_PROPERTY(sdei, dyn_config, private_ev_intrs[i]),
+			FCONF_GET_PROPERTY(sdei, dyn_config, private_ev_flags[i]));
+	}
+
+	for (i = 0; i < FCONF_GET_PROPERTY(sdei, dyn_config, shared_ev_cnt); i++) {
+		arm_sdei_shared[i] = (sdei_ev_map_t)SDEI_SHARED_EVENT( \
+			FCONF_GET_PROPERTY(sdei, dyn_config, shared_ev_nums[i]),
+			FCONF_GET_PROPERTY(sdei, dyn_config, shared_ev_intrs[i]),
+			FCONF_GET_PROPERTY(sdei, dyn_config, shared_ev_flags[i]));
+	}
+	INFO("FCONF: SDEI platform setup\n");
+}
+#else
+/* Private event mappings */
+static sdei_ev_map_t arm_sdei_private[] = {
+	PLAT_ARM_PRIVATE_SDEI_EVENTS
+};
+
+/* Shared event mappings */
+static sdei_ev_map_t arm_sdei_shared[] = {
+	PLAT_ARM_SHARED_SDEI_EVENTS
+};
+
+void plat_sdei_setup(void)
+{
+	INFO("SDEI platform setup\n");
+}
+#endif /* SDEI_IN_FCONF */
+
+/* Export ARM SDEI events */
+REGISTER_SDEI_MAP(arm_sdei_private, arm_sdei_shared);

source/trusted-firmware/trusted-firmware-a-2.8/plat/arm/common/aarch64/execution_state_switch.c

@@ -0,0 +1,180 @@
+/*
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <context.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/psci/psci.h>
+#include <lib/utils.h>
+#include <plat/arm/common/plat_arm.h>
+#include <smccc_helpers.h>
+
+/*
+ * Handle SMC from a lower exception level to switch its execution state
+ * (either from AArch64 to AArch32, or vice versa).
+ *
+ * smc_fid:
+ *	SMC function ID - either ARM_SIP_SVC_STATE_SWITCH_64 or
+ *	ARM_SIP_SVC_STATE_SWITCH_32.
+ * pc_hi, pc_lo:
+ *	PC upon re-entry to the calling exception level; width dependent on the
+ *	calling exception level.
+ * cookie_hi, cookie_lo:
+ *	Opaque pointer pairs received from the caller to pass it back, upon
+ *	re-entry.
+ * handle:
+ *	Handle to saved context.
+ */
+int arm_execution_state_switch(unsigned int smc_fid,
+		uint32_t pc_hi,
+		uint32_t pc_lo,
+		uint32_t cookie_hi,
+		uint32_t cookie_lo,
+		void *handle)
+{
+	bool caller_64, thumb = false, from_el2;
+	unsigned int el, endianness;
+	u_register_t spsr, pc, scr, sctlr;
+	entry_point_info_t ep;
+	cpu_context_t *ctx = (cpu_context_t *) handle;
+	el3_state_t *el3_ctx = get_el3state_ctx(ctx);
+
+	/* Validate supplied entry point */
+	pc = (u_register_t) (((uint64_t) pc_hi << 32) | pc_lo);
+	if (arm_validate_ns_entrypoint(pc) != 0)
+		goto invalid_param;
+
+	/* That the SMC originated from NS is already validated by the caller */
+
+	/*
+	 * Disallow state switch if any of the secondaries have been brought up.
+	 */
+	if (psci_secondaries_brought_up() != 0)
+		goto exec_denied;
+
+	spsr = read_ctx_reg(el3_ctx, CTX_SPSR_EL3);
+	caller_64 = (GET_RW(spsr) == MODE_RW_64);
+
+	if (caller_64) {
+		/*
+		 * If the call originated from AArch64, expect 32-bit pointers when
+		 * switching to AArch32.
+		 */
+		if ((pc_hi != 0U) || (cookie_hi != 0U))
+			goto invalid_param;
+
+		pc = pc_lo;
+
+		/* Instruction state when entering AArch32 */
+		thumb = (pc & 1U) != 0U;
+	} else {
+		/* Construct AArch64 PC */
+		pc = (((u_register_t) pc_hi) << 32) | pc_lo;
+	}
+
+	/* Make sure PC is 4-byte aligned, except for Thumb */
+	if (((pc & 0x3U) != 0U) && !thumb)
+		goto invalid_param;
+
+	/*
+	 * EL3 controls register width of the immediate lower EL only. Expect
+	 * this request from EL2/Hyp unless:
+	 *
+	 * - EL2 is not implemented;
+	 * - EL2 is implemented, but was disabled. This can be inferred from
+	 *   SCR_EL3.HCE.
+	 */
+	from_el2 = caller_64 ? (GET_EL(spsr) == MODE_EL2) :
+		(GET_M32(spsr) == MODE32_hyp);
+	scr = read_ctx_reg(el3_ctx, CTX_SCR_EL3);
+	if (!from_el2) {
+		/* The call is from NS privilege level other than HYP */
+
+		/*
+		 * Disallow switching state if there's a Hypervisor in place;
+		 * this request must be taken up with the Hypervisor instead.
+		 */
+		if ((scr & SCR_HCE_BIT) != 0U)
+			goto exec_denied;
+	}
+
+	/*
+	 * Return to the caller using the same endianness. Extract
+	 * endianness bit from the respective system control register
+	 * directly.
+	 */
+	sctlr = from_el2 ? read_sctlr_el2() : read_sctlr_el1();
+	endianness = ((sctlr & SCTLR_EE_BIT) != 0U) ? 1U : 0U;
+
+	/* Construct SPSR for the exception state we're about to switch to */
+	if (caller_64) {
+		unsigned long long impl;
+
+		/*
+		 * Switching from AArch64 to AArch32. Ensure this CPU implements
+		 * the target EL in AArch32.
+		 */
+		impl = from_el2 ? el_implemented(2) : el_implemented(1);
+		if (impl != EL_IMPL_A64_A32)
+			goto exec_denied;
+
+		/* Return to the equivalent AArch32 privilege level */
+		el = from_el2 ? MODE32_hyp : MODE32_svc;
+		spsr = SPSR_MODE32((u_register_t) el,
+				thumb ? SPSR_T_THUMB : SPSR_T_ARM,
+				endianness, DISABLE_ALL_EXCEPTIONS);
+	} else {
+		/*
+		 * Switching from AArch32 to AArch64. Since it's not possible to
+		 * implement an EL as AArch32-only (from which this call was
+		 * raised), it's safe to assume AArch64 is also implemented.
+		 */
+		el = from_el2 ? MODE_EL2 : MODE_EL1;
+		spsr = SPSR_64((u_register_t) el, MODE_SP_ELX,
+				DISABLE_ALL_EXCEPTIONS);
+	}
+
+	/*
+	 * Use the context management library to re-initialize the existing
+	 * context with the execution state flipped. Since the library takes
+	 * entry_point_info_t pointer as the argument, construct a dummy one
+	 * with PC, state width, endianness, security etc. appropriately set.
+	 * Other entries in the entry point structure are irrelevant for
+	 * purpose.
+	 */
+	zeromem(&ep, sizeof(ep));
+	ep.pc = pc;
+	ep.spsr = (uint32_t) spsr;
+	SET_PARAM_HEAD(&ep, PARAM_EP, VERSION_1,
+			((unsigned int) ((endianness != 0U) ? EP_EE_BIG :
+				EP_EE_LITTLE)
+			 | NON_SECURE | EP_ST_DISABLE));
+
+	/*
+	 * Re-initialize the system register context, and exit EL3 as if for the
+	 * first time. State switch is effectively a soft reset of the
+	 * calling EL.
+	 */
+	cm_init_my_context(&ep);
+	cm_prepare_el3_exit_ns();
+
+	/*
+	 * State switch success. The caller of SMC wouldn't see the SMC
+	 * returning. Instead, execution starts at the supplied entry point,
+	 * with context pointers populated in registers 0 and 1.
+	 */
+	SMC_RET2(handle, cookie_hi, cookie_lo);
+
+invalid_param:
+	SMC_RET1(handle, STATE_SW_E_PARAM);
+
+exec_denied:
+	/* State switch denied */
+	SMC_RET1(handle, STATE_SW_E_DENIED);
+}