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

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/opteed/opteed.mk

@@ -0,0 +1,18 @@
+#
+# Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+OPTEED_DIR		:=	services/spd/opteed
+SPD_INCLUDES		:=
+
+SPD_SOURCES		:=	services/spd/opteed/opteed_common.c	\
+				services/spd/opteed/opteed_helpers.S	\
+				services/spd/opteed/opteed_main.c	\
+				services/spd/opteed/opteed_pm.c
+
+NEED_BL32		:=	yes
+
+# required so that optee code can control access to the timer registers
+NS_TIMER_SWITCH		:=	1

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/opteed/opteed_common.c

@@ -0,0 +1,111 @@
+/*
+ * Copyright (c) 2013-2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/utils.h>
+
+#include "opteed_private.h"
+
+/*******************************************************************************
+ * Given a OPTEE entrypoint info pointer, entry point PC, register width,
+ * cpu id & pointer to a context data structure, this function will
+ * initialize OPTEE context and entry point info for OPTEE.
+ ******************************************************************************/
+void opteed_init_optee_ep_state(struct entry_point_info *optee_entry_point,
+				uint32_t rw, uint64_t pc,
+				uint64_t pageable_part, uint64_t mem_limit,
+				uint64_t dt_addr, optee_context_t *optee_ctx)
+{
+	uint32_t ep_attr;
+
+	/* Passing a NULL context is a critical programming error */
+	assert(optee_ctx);
+	assert(optee_entry_point);
+	assert(pc);
+
+	/* Associate this context with the cpu specified */
+	optee_ctx->mpidr = read_mpidr_el1();
+	optee_ctx->state = 0;
+	set_optee_pstate(optee_ctx->state, OPTEE_PSTATE_OFF);
+
+	cm_set_context(&optee_ctx->cpu_ctx, SECURE);
+
+	/* initialise an entrypoint to set up the CPU context */
+	ep_attr = SECURE | EP_ST_ENABLE;
+	if (read_sctlr_el3() & SCTLR_EE_BIT)
+		ep_attr |= EP_EE_BIG;
+	SET_PARAM_HEAD(optee_entry_point, PARAM_EP, VERSION_1, ep_attr);
+	optee_entry_point->pc = pc;
+	if (rw == OPTEE_AARCH64)
+		optee_entry_point->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX,
+						  DISABLE_ALL_EXCEPTIONS);
+	else
+		optee_entry_point->spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM,
+						      SPSR_E_LITTLE,
+						      DAIF_FIQ_BIT |
+							DAIF_IRQ_BIT |
+							DAIF_ABT_BIT);
+	zeromem(&optee_entry_point->args, sizeof(optee_entry_point->args));
+	optee_entry_point->args.arg0 = pageable_part;
+	optee_entry_point->args.arg1 = mem_limit;
+	optee_entry_point->args.arg2 = dt_addr;
+}
+
+/*******************************************************************************
+ * This function takes an OPTEE context pointer and:
+ * 1. Applies the S-EL1 system register context from optee_ctx->cpu_ctx.
+ * 2. Saves the current C runtime state (callee saved registers) on the stack
+ *    frame and saves a reference to this state.
+ * 3. Calls el3_exit() so that the EL3 system and general purpose registers
+ *    from the optee_ctx->cpu_ctx are used to enter the OPTEE image.
+ ******************************************************************************/
+uint64_t opteed_synchronous_sp_entry(optee_context_t *optee_ctx)
+{
+	uint64_t rc;
+
+	assert(optee_ctx != NULL);
+	assert(optee_ctx->c_rt_ctx == 0);
+
+	/* Apply the Secure EL1 system register context and switch to it */
+	assert(cm_get_context(SECURE) == &optee_ctx->cpu_ctx);
+	cm_el1_sysregs_context_restore(SECURE);
+	cm_set_next_eret_context(SECURE);
+
+	rc = opteed_enter_sp(&optee_ctx->c_rt_ctx);
+#if ENABLE_ASSERTIONS
+	optee_ctx->c_rt_ctx = 0;
+#endif
+
+	return rc;
+}
+
+
+/*******************************************************************************
+ * This function takes an OPTEE context pointer and:
+ * 1. Saves the S-EL1 system register context tp optee_ctx->cpu_ctx.
+ * 2. Restores the current C runtime state (callee saved registers) from the
+ *    stack frame using the reference to this state saved in opteed_enter_sp().
+ * 3. It does not need to save any general purpose or EL3 system register state
+ *    as the generic smc entry routine should have saved those.
+ ******************************************************************************/
+void opteed_synchronous_sp_exit(optee_context_t *optee_ctx, uint64_t ret)
+{
+	assert(optee_ctx != NULL);
+	/* Save the Secure EL1 system register context */
+	assert(cm_get_context(SECURE) == &optee_ctx->cpu_ctx);
+	cm_el1_sysregs_context_save(SECURE);
+
+	assert(optee_ctx->c_rt_ctx != 0);
+	opteed_exit_sp(optee_ctx->c_rt_ctx, ret);
+
+	/* Should never reach here */
+	assert(0);
+}

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/opteed/opteed_helpers.S

@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+#include "opteed_private.h"
+
+	.global	opteed_enter_sp
+	/* ---------------------------------------------
+	 * This function is called with SP_EL0 as stack.
+	 * Here we stash our EL3 callee-saved registers
+	 * on to the stack as a part of saving the C
+	 * runtime and enter the secure payload.
+	 * 'x0' contains a pointer to the memory where
+	 * the address of the C runtime context is to be
+	 * saved.
+	 * ---------------------------------------------
+	 */
+func opteed_enter_sp
+	/* Make space for the registers that we're going to save */
+	mov	x3, sp
+	str	x3, [x0, #0]
+	sub	sp, sp, #OPTEED_C_RT_CTX_SIZE
+
+	/* Save callee-saved registers on to the stack */
+	stp	x19, x20, [sp, #OPTEED_C_RT_CTX_X19]
+	stp	x21, x22, [sp, #OPTEED_C_RT_CTX_X21]
+	stp	x23, x24, [sp, #OPTEED_C_RT_CTX_X23]
+	stp	x25, x26, [sp, #OPTEED_C_RT_CTX_X25]
+	stp	x27, x28, [sp, #OPTEED_C_RT_CTX_X27]
+	stp	x29, x30, [sp, #OPTEED_C_RT_CTX_X29]
+
+	/* ---------------------------------------------
+	 * Everything is setup now. el3_exit() will
+	 * use the secure context to restore to the
+	 * general purpose and EL3 system registers to
+	 * ERET into OPTEE.
+	 * ---------------------------------------------
+	 */
+	b	el3_exit
+endfunc opteed_enter_sp
+
+	/* ---------------------------------------------
+	 * This function is called 'x0' pointing to a C
+	 * runtime context saved in opteed_enter_sp().  It
+	 * restores the saved registers and jumps to
+	 * that runtime with 'x0' as the new sp. This
+	 * destroys the C runtime context that had been
+	 * built on the stack below the saved context by
+	 * the caller. Later the second parameter 'x1'
+	 * is passed as return value to the caller
+	 * ---------------------------------------------
+	 */
+	.global opteed_exit_sp
+func opteed_exit_sp
+	/* Restore the previous stack */
+	mov	sp, x0
+
+	/* Restore callee-saved registers on to the stack */
+	ldp	x19, x20, [x0, #(OPTEED_C_RT_CTX_X19 - OPTEED_C_RT_CTX_SIZE)]
+	ldp	x21, x22, [x0, #(OPTEED_C_RT_CTX_X21 - OPTEED_C_RT_CTX_SIZE)]
+	ldp	x23, x24, [x0, #(OPTEED_C_RT_CTX_X23 - OPTEED_C_RT_CTX_SIZE)]
+	ldp	x25, x26, [x0, #(OPTEED_C_RT_CTX_X25 - OPTEED_C_RT_CTX_SIZE)]
+	ldp	x27, x28, [x0, #(OPTEED_C_RT_CTX_X27 - OPTEED_C_RT_CTX_SIZE)]
+	ldp	x29, x30, [x0, #(OPTEED_C_RT_CTX_X29 - OPTEED_C_RT_CTX_SIZE)]
+
+	/* ---------------------------------------------
+	 * This should take us back to the instruction
+	 * after the call to the last opteed_enter_sp().
+	 * Place the second parameter to x0 so that the
+	 * caller will see it as a return value from the
+	 * original entry call
+	 * ---------------------------------------------
+	 */
+	mov	x0, x1
+	ret
+endfunc opteed_exit_sp

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/opteed/opteed_main.c

@@ -0,0 +1,420 @@
+/*
+ * Copyright (c) 2013-2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+
+/*******************************************************************************
+ * This is the Secure Payload Dispatcher (SPD). The dispatcher is meant to be a
+ * plug-in component to the Secure Monitor, registered as a runtime service. The
+ * SPD is expected to be a functional extension of the Secure Payload (SP) that
+ * executes in Secure EL1. The Secure Monitor will delegate all SMCs targeting
+ * the Trusted OS/Applications range to the dispatcher. The SPD will either
+ * handle the request locally or delegate it to the Secure Payload. It is also
+ * responsible for initialising and maintaining communication with the SP.
+ ******************************************************************************/
+#include <assert.h>
+#include <errno.h>
+#include <stddef.h>
+
+#include <arch_helpers.h>
+#include <bl31/bl31.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <plat/common/platform.h>
+#include <tools_share/uuid.h>
+
+#include "opteed_private.h"
+#include "teesmc_opteed.h"
+#include "teesmc_opteed_macros.h"
+
+/*******************************************************************************
+ * Address of the entrypoint vector table in OPTEE. It is
+ * initialised once on the primary core after a cold boot.
+ ******************************************************************************/
+struct optee_vectors *optee_vector_table;
+
+/*******************************************************************************
+ * Array to keep track of per-cpu OPTEE state
+ ******************************************************************************/
+optee_context_t opteed_sp_context[OPTEED_CORE_COUNT];
+uint32_t opteed_rw;
+
+static int32_t opteed_init(void);
+
+/*******************************************************************************
+ * This function is the handler registered for S-EL1 interrupts by the
+ * OPTEED. It validates the interrupt and upon success arranges entry into
+ * the OPTEE at 'optee_fiq_entry()' for handling the interrupt.
+ ******************************************************************************/
+static uint64_t opteed_sel1_interrupt_handler(uint32_t id,
+					    uint32_t flags,
+					    void *handle,
+					    void *cookie)
+{
+	uint32_t linear_id;
+	optee_context_t *optee_ctx;
+
+	/* Check the security state when the exception was generated */
+	assert(get_interrupt_src_ss(flags) == NON_SECURE);
+
+	/* Sanity check the pointer to this cpu's context */
+	assert(handle == cm_get_context(NON_SECURE));
+
+	/* Save the non-secure context before entering the OPTEE */
+	cm_el1_sysregs_context_save(NON_SECURE);
+
+	/* Get a reference to this cpu's OPTEE context */
+	linear_id = plat_my_core_pos();
+	optee_ctx = &opteed_sp_context[linear_id];
+	assert(&optee_ctx->cpu_ctx == cm_get_context(SECURE));
+
+	cm_set_elr_el3(SECURE, (uint64_t)&optee_vector_table->fiq_entry);
+	cm_el1_sysregs_context_restore(SECURE);
+	cm_set_next_eret_context(SECURE);
+
+	/*
+	 * Tell the OPTEE that it has to handle an FIQ (synchronously).
+	 * Also the instruction in normal world where the interrupt was
+	 * generated is passed for debugging purposes. It is safe to
+	 * retrieve this address from ELR_EL3 as the secure context will
+	 * not take effect until el3_exit().
+	 */
+	SMC_RET1(&optee_ctx->cpu_ctx, read_elr_el3());
+}
+
+/*******************************************************************************
+ * OPTEE Dispatcher setup. The OPTEED finds out the OPTEE entrypoint and type
+ * (aarch32/aarch64) if not already known and initialises the context for entry
+ * into OPTEE for its initialization.
+ ******************************************************************************/
+static int32_t opteed_setup(void)
+{
+	entry_point_info_t *optee_ep_info;
+	uint32_t linear_id;
+	uint64_t opteed_pageable_part;
+	uint64_t opteed_mem_limit;
+	uint64_t dt_addr;
+
+	linear_id = plat_my_core_pos();
+
+	/*
+	 * Get information about the Secure Payload (BL32) image. Its
+	 * absence is a critical failure.  TODO: Add support to
+	 * conditionally include the SPD service
+	 */
+	optee_ep_info = bl31_plat_get_next_image_ep_info(SECURE);
+	if (!optee_ep_info) {
+		WARN("No OPTEE provided by BL2 boot loader, Booting device"
+			" without OPTEE initialization. SMC`s destined for OPTEE"
+			" will return SMC_UNK\n");
+		return 1;
+	}
+
+	/*
+	 * If there's no valid entry point for SP, we return a non-zero value
+	 * signalling failure initializing the service. We bail out without
+	 * registering any handlers
+	 */
+	if (!optee_ep_info->pc)
+		return 1;
+
+	opteed_rw = optee_ep_info->args.arg0;
+	opteed_pageable_part = optee_ep_info->args.arg1;
+	opteed_mem_limit = optee_ep_info->args.arg2;
+	dt_addr = optee_ep_info->args.arg3;
+
+	opteed_init_optee_ep_state(optee_ep_info,
+				opteed_rw,
+				optee_ep_info->pc,
+				opteed_pageable_part,
+				opteed_mem_limit,
+				dt_addr,
+				&opteed_sp_context[linear_id]);
+
+	/*
+	 * All OPTEED initialization done. Now register our init function with
+	 * BL31 for deferred invocation
+	 */
+	bl31_register_bl32_init(&opteed_init);
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function passes control to the OPTEE image (BL32) for the first time
+ * on the primary cpu after a cold boot. It assumes that a valid secure
+ * context has already been created by opteed_setup() which can be directly
+ * used.  It also assumes that a valid non-secure context has been
+ * initialised by PSCI so it does not need to save and restore any
+ * non-secure state. This function performs a synchronous entry into
+ * OPTEE. OPTEE passes control back to this routine through a SMC.
+ ******************************************************************************/
+static int32_t opteed_init(void)
+{
+	uint32_t linear_id = plat_my_core_pos();
+	optee_context_t *optee_ctx = &opteed_sp_context[linear_id];
+	entry_point_info_t *optee_entry_point;
+	uint64_t rc;
+
+	/*
+	 * Get information about the OPTEE (BL32) image. Its
+	 * absence is a critical failure.
+	 */
+	optee_entry_point = bl31_plat_get_next_image_ep_info(SECURE);
+	assert(optee_entry_point);
+
+	cm_init_my_context(optee_entry_point);
+
+	/*
+	 * Arrange for an entry into OPTEE. It will be returned via
+	 * OPTEE_ENTRY_DONE case
+	 */
+	rc = opteed_synchronous_sp_entry(optee_ctx);
+	assert(rc != 0);
+
+	return rc;
+}
+
+
+/*******************************************************************************
+ * This function is responsible for handling all SMCs in the Trusted OS/App
+ * range from the non-secure state as defined in the SMC Calling Convention
+ * Document. It is also responsible for communicating with the Secure
+ * payload to delegate work and return results back to the non-secure
+ * state. Lastly it will also return any information that OPTEE needs to do
+ * the work assigned to it.
+ ******************************************************************************/
+static uintptr_t opteed_smc_handler(uint32_t smc_fid,
+			 u_register_t x1,
+			 u_register_t x2,
+			 u_register_t x3,
+			 u_register_t x4,
+			 void *cookie,
+			 void *handle,
+			 u_register_t flags)
+{
+	cpu_context_t *ns_cpu_context;
+	uint32_t linear_id = plat_my_core_pos();
+	optee_context_t *optee_ctx = &opteed_sp_context[linear_id];
+	uint64_t rc;
+
+	/*
+	 * Determine which security state this SMC originated from
+	 */
+
+	if (is_caller_non_secure(flags)) {
+		/*
+		 * This is a fresh request from the non-secure client.
+		 * The parameters are in x1 and x2. Figure out which
+		 * registers need to be preserved, save the non-secure
+		 * state and send the request to the secure payload.
+		 */
+		assert(handle == cm_get_context(NON_SECURE));
+
+		cm_el1_sysregs_context_save(NON_SECURE);
+
+		/*
+		 * We are done stashing the non-secure context. Ask the
+		 * OPTEE to do the work now.
+		 */
+
+		/*
+		 * Verify if there is a valid context to use, copy the
+		 * operation type and parameters to the secure context
+		 * and jump to the fast smc entry point in the secure
+		 * payload. Entry into S-EL1 will take place upon exit
+		 * from this function.
+		 */
+		assert(&optee_ctx->cpu_ctx == cm_get_context(SECURE));
+
+		/* Set appropriate entry for SMC.
+		 * We expect OPTEE to manage the PSTATE.I and PSTATE.F
+		 * flags as appropriate.
+		 */
+		if (GET_SMC_TYPE(smc_fid) == SMC_TYPE_FAST) {
+			cm_set_elr_el3(SECURE, (uint64_t)
+					&optee_vector_table->fast_smc_entry);
+		} else {
+			cm_set_elr_el3(SECURE, (uint64_t)
+					&optee_vector_table->yield_smc_entry);
+		}
+
+		cm_el1_sysregs_context_restore(SECURE);
+		cm_set_next_eret_context(SECURE);
+
+		write_ctx_reg(get_gpregs_ctx(&optee_ctx->cpu_ctx),
+			      CTX_GPREG_X4,
+			      read_ctx_reg(get_gpregs_ctx(handle),
+					   CTX_GPREG_X4));
+		write_ctx_reg(get_gpregs_ctx(&optee_ctx->cpu_ctx),
+			      CTX_GPREG_X5,
+			      read_ctx_reg(get_gpregs_ctx(handle),
+					   CTX_GPREG_X5));
+		write_ctx_reg(get_gpregs_ctx(&optee_ctx->cpu_ctx),
+			      CTX_GPREG_X6,
+			      read_ctx_reg(get_gpregs_ctx(handle),
+					   CTX_GPREG_X6));
+		/* Propagate hypervisor client ID */
+		write_ctx_reg(get_gpregs_ctx(&optee_ctx->cpu_ctx),
+			      CTX_GPREG_X7,
+			      read_ctx_reg(get_gpregs_ctx(handle),
+					   CTX_GPREG_X7));
+
+		SMC_RET4(&optee_ctx->cpu_ctx, smc_fid, x1, x2, x3);
+	}
+
+	/*
+	 * Returning from OPTEE
+	 */
+
+	switch (smc_fid) {
+	/*
+	 * OPTEE has finished initialising itself after a cold boot
+	 */
+	case TEESMC_OPTEED_RETURN_ENTRY_DONE:
+		/*
+		 * Stash the OPTEE entry points information. This is done
+		 * only once on the primary cpu
+		 */
+		assert(optee_vector_table == NULL);
+		optee_vector_table = (optee_vectors_t *) x1;
+
+		if (optee_vector_table) {
+			set_optee_pstate(optee_ctx->state, OPTEE_PSTATE_ON);
+
+			/*
+			 * OPTEE has been successfully initialized.
+			 * Register power management hooks with PSCI
+			 */
+			psci_register_spd_pm_hook(&opteed_pm);
+
+			/*
+			 * Register an interrupt handler for S-EL1 interrupts
+			 * when generated during code executing in the
+			 * non-secure state.
+			 */
+			flags = 0;
+			set_interrupt_rm_flag(flags, NON_SECURE);
+			rc = register_interrupt_type_handler(INTR_TYPE_S_EL1,
+						opteed_sel1_interrupt_handler,
+						flags);
+			if (rc)
+				panic();
+		}
+
+		/*
+		 * OPTEE reports completion. The OPTEED must have initiated
+		 * the original request through a synchronous entry into
+		 * OPTEE. Jump back to the original C runtime context.
+		 */
+		opteed_synchronous_sp_exit(optee_ctx, x1);
+		break;
+
+
+	/*
+	 * These function IDs is used only by OP-TEE to indicate it has
+	 * finished:
+	 * 1. turning itself on in response to an earlier psci
+	 *    cpu_on request
+	 * 2. resuming itself after an earlier psci cpu_suspend
+	 *    request.
+	 */
+	case TEESMC_OPTEED_RETURN_ON_DONE:
+	case TEESMC_OPTEED_RETURN_RESUME_DONE:
+
+
+	/*
+	 * These function IDs is used only by the SP to indicate it has
+	 * finished:
+	 * 1. suspending itself after an earlier psci cpu_suspend
+	 *    request.
+	 * 2. turning itself off in response to an earlier psci
+	 *    cpu_off request.
+	 */
+	case TEESMC_OPTEED_RETURN_OFF_DONE:
+	case TEESMC_OPTEED_RETURN_SUSPEND_DONE:
+	case TEESMC_OPTEED_RETURN_SYSTEM_OFF_DONE:
+	case TEESMC_OPTEED_RETURN_SYSTEM_RESET_DONE:
+
+		/*
+		 * OPTEE reports completion. The OPTEED must have initiated the
+		 * original request through a synchronous entry into OPTEE.
+		 * Jump back to the original C runtime context, and pass x1 as
+		 * return value to the caller
+		 */
+		opteed_synchronous_sp_exit(optee_ctx, x1);
+		break;
+
+	/*
+	 * OPTEE is returning from a call or being preempted from a call, in
+	 * either case execution should resume in the normal world.
+	 */
+	case TEESMC_OPTEED_RETURN_CALL_DONE:
+		/*
+		 * This is the result from the secure client of an
+		 * earlier request. The results are in x0-x3. Copy it
+		 * into the non-secure context, save the secure state
+		 * and return to the non-secure state.
+		 */
+		assert(handle == cm_get_context(SECURE));
+		cm_el1_sysregs_context_save(SECURE);
+
+		/* Get a reference to the non-secure context */
+		ns_cpu_context = cm_get_context(NON_SECURE);
+		assert(ns_cpu_context);
+
+		/* Restore non-secure state */
+		cm_el1_sysregs_context_restore(NON_SECURE);
+		cm_set_next_eret_context(NON_SECURE);
+
+		SMC_RET4(ns_cpu_context, x1, x2, x3, x4);
+
+	/*
+	 * OPTEE has finished handling a S-EL1 FIQ interrupt. Execution
+	 * should resume in the normal world.
+	 */
+	case TEESMC_OPTEED_RETURN_FIQ_DONE:
+		/* Get a reference to the non-secure context */
+		ns_cpu_context = cm_get_context(NON_SECURE);
+		assert(ns_cpu_context);
+
+		/*
+		 * Restore non-secure state. There is no need to save the
+		 * secure system register context since OPTEE was supposed
+		 * to preserve it during S-EL1 interrupt handling.
+		 */
+		cm_el1_sysregs_context_restore(NON_SECURE);
+		cm_set_next_eret_context(NON_SECURE);
+
+		SMC_RET0((uint64_t) ns_cpu_context);
+
+	default:
+		panic();
+	}
+}
+
+/* Define an OPTEED runtime service descriptor for fast SMC calls */
+DECLARE_RT_SVC(
+	opteed_fast,
+
+	OEN_TOS_START,
+	OEN_TOS_END,
+	SMC_TYPE_FAST,
+	opteed_setup,
+	opteed_smc_handler
+);
+
+/* Define an OPTEED runtime service descriptor for yielding SMC calls */
+DECLARE_RT_SVC(
+	opteed_std,
+
+	OEN_TOS_START,
+	OEN_TOS_END,
+	SMC_TYPE_YIELD,
+	NULL,
+	opteed_smc_handler
+);

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/opteed/opteed_pm.c

@@ -0,0 +1,223 @@
+/*
+ * Copyright (c) 2013-2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <plat/common/platform.h>
+
+#include "opteed_private.h"
+
+/*******************************************************************************
+ * The target cpu is being turned on. Allow the OPTEED/OPTEE to perform any
+ * actions needed. Nothing at the moment.
+ ******************************************************************************/
+static void opteed_cpu_on_handler(u_register_t target_cpu)
+{
+}
+
+/*******************************************************************************
+ * This cpu is being turned off. Allow the OPTEED/OPTEE to perform any actions
+ * needed
+ ******************************************************************************/
+static int32_t opteed_cpu_off_handler(u_register_t unused)
+{
+	int32_t rc = 0;
+	uint32_t linear_id = plat_my_core_pos();
+	optee_context_t *optee_ctx = &opteed_sp_context[linear_id];
+
+	assert(optee_vector_table);
+	assert(get_optee_pstate(optee_ctx->state) == OPTEE_PSTATE_ON);
+
+	/* Program the entry point and enter OPTEE */
+	cm_set_elr_el3(SECURE, (uint64_t) &optee_vector_table->cpu_off_entry);
+	rc = opteed_synchronous_sp_entry(optee_ctx);
+
+	/*
+	 * Read the response from OPTEE. A non-zero return means that
+	 * something went wrong while communicating with OPTEE.
+	 */
+	if (rc != 0)
+		panic();
+
+	/*
+	 * Reset OPTEE's context for a fresh start when this cpu is turned on
+	 * subsequently.
+	 */
+	set_optee_pstate(optee_ctx->state, OPTEE_PSTATE_OFF);
+
+	 return 0;
+}
+
+/*******************************************************************************
+ * This cpu is being suspended. S-EL1 state must have been saved in the
+ * resident cpu (mpidr format) if it is a UP/UP migratable OPTEE.
+ ******************************************************************************/
+static void opteed_cpu_suspend_handler(u_register_t max_off_pwrlvl)
+{
+	int32_t rc = 0;
+	uint32_t linear_id = plat_my_core_pos();
+	optee_context_t *optee_ctx = &opteed_sp_context[linear_id];
+
+	assert(optee_vector_table);
+	assert(get_optee_pstate(optee_ctx->state) == OPTEE_PSTATE_ON);
+
+	write_ctx_reg(get_gpregs_ctx(&optee_ctx->cpu_ctx), CTX_GPREG_X0,
+		      max_off_pwrlvl);
+
+	/* Program the entry point and enter OPTEE */
+	cm_set_elr_el3(SECURE, (uint64_t) &optee_vector_table->cpu_suspend_entry);
+	rc = opteed_synchronous_sp_entry(optee_ctx);
+
+	/*
+	 * Read the response from OPTEE. A non-zero return means that
+	 * something went wrong while communicating with OPTEE.
+	 */
+	if (rc != 0)
+		panic();
+
+	/* Update its context to reflect the state OPTEE is in */
+	set_optee_pstate(optee_ctx->state, OPTEE_PSTATE_SUSPEND);
+}
+
+/*******************************************************************************
+ * This cpu has been turned on. Enter OPTEE to initialise S-EL1 and other bits
+ * before passing control back to the Secure Monitor. Entry in S-El1 is done
+ * after initialising minimal architectural state that guarantees safe
+ * execution.
+ ******************************************************************************/
+static void opteed_cpu_on_finish_handler(u_register_t unused)
+{
+	int32_t rc = 0;
+	uint32_t linear_id = plat_my_core_pos();
+	optee_context_t *optee_ctx = &opteed_sp_context[linear_id];
+	entry_point_info_t optee_on_entrypoint;
+
+	assert(optee_vector_table);
+	assert(get_optee_pstate(optee_ctx->state) == OPTEE_PSTATE_OFF);
+
+	opteed_init_optee_ep_state(&optee_on_entrypoint, opteed_rw,
+				(uint64_t)&optee_vector_table->cpu_on_entry,
+				0, 0, 0, optee_ctx);
+
+	/* Initialise this cpu's secure context */
+	cm_init_my_context(&optee_on_entrypoint);
+
+	/* Enter OPTEE */
+	rc = opteed_synchronous_sp_entry(optee_ctx);
+
+	/*
+	 * Read the response from OPTEE. A non-zero return means that
+	 * something went wrong while communicating with OPTEE.
+	 */
+	if (rc != 0)
+		panic();
+
+	/* Update its context to reflect the state OPTEE is in */
+	set_optee_pstate(optee_ctx->state, OPTEE_PSTATE_ON);
+}
+
+/*******************************************************************************
+ * This cpu has resumed from suspend. The OPTEED saved the OPTEE context when it
+ * completed the preceding suspend call. Use that context to program an entry
+ * into OPTEE to allow it to do any remaining book keeping
+ ******************************************************************************/
+static void opteed_cpu_suspend_finish_handler(u_register_t max_off_pwrlvl)
+{
+	int32_t rc = 0;
+	uint32_t linear_id = plat_my_core_pos();
+	optee_context_t *optee_ctx = &opteed_sp_context[linear_id];
+
+	assert(optee_vector_table);
+	assert(get_optee_pstate(optee_ctx->state) == OPTEE_PSTATE_SUSPEND);
+
+	/* Program the entry point, max_off_pwrlvl and enter the SP */
+	write_ctx_reg(get_gpregs_ctx(&optee_ctx->cpu_ctx),
+		      CTX_GPREG_X0,
+		      max_off_pwrlvl);
+	cm_set_elr_el3(SECURE, (uint64_t) &optee_vector_table->cpu_resume_entry);
+	rc = opteed_synchronous_sp_entry(optee_ctx);
+
+	/*
+	 * Read the response from OPTEE. A non-zero return means that
+	 * something went wrong while communicating with OPTEE.
+	 */
+	if (rc != 0)
+		panic();
+
+	/* Update its context to reflect the state OPTEE is in */
+	set_optee_pstate(optee_ctx->state, OPTEE_PSTATE_ON);
+}
+
+/*******************************************************************************
+ * Return the type of OPTEE the OPTEED is dealing with. Report the current
+ * resident cpu (mpidr format) if it is a UP/UP migratable OPTEE.
+ ******************************************************************************/
+static int32_t opteed_cpu_migrate_info(u_register_t *resident_cpu)
+{
+	return OPTEE_MIGRATE_INFO;
+}
+
+/*******************************************************************************
+ * System is about to be switched off. Allow the OPTEED/OPTEE to perform
+ * any actions needed.
+ ******************************************************************************/
+static void opteed_system_off(void)
+{
+	uint32_t linear_id = plat_my_core_pos();
+	optee_context_t *optee_ctx = &opteed_sp_context[linear_id];
+
+	assert(optee_vector_table);
+	assert(get_optee_pstate(optee_ctx->state) == OPTEE_PSTATE_ON);
+
+	/* Program the entry point */
+	cm_set_elr_el3(SECURE, (uint64_t) &optee_vector_table->system_off_entry);
+
+	/* Enter OPTEE. We do not care about the return value because we
+	 * must continue the shutdown anyway */
+	opteed_synchronous_sp_entry(optee_ctx);
+}
+
+/*******************************************************************************
+ * System is about to be reset. Allow the OPTEED/OPTEE to perform
+ * any actions needed.
+ ******************************************************************************/
+static void opteed_system_reset(void)
+{
+	uint32_t linear_id = plat_my_core_pos();
+	optee_context_t *optee_ctx = &opteed_sp_context[linear_id];
+
+	assert(optee_vector_table);
+	assert(get_optee_pstate(optee_ctx->state) == OPTEE_PSTATE_ON);
+
+	/* Program the entry point */
+	cm_set_elr_el3(SECURE, (uint64_t) &optee_vector_table->system_reset_entry);
+
+	/* Enter OPTEE. We do not care about the return value because we
+	 * must continue the reset anyway */
+	opteed_synchronous_sp_entry(optee_ctx);
+}
+
+
+/*******************************************************************************
+ * Structure populated by the OPTEE Dispatcher to be given a chance to
+ * perform any OPTEE bookkeeping before PSCI executes a power mgmt.
+ * operation.
+ ******************************************************************************/
+const spd_pm_ops_t opteed_pm = {
+	.svc_on = opteed_cpu_on_handler,
+	.svc_off = opteed_cpu_off_handler,
+	.svc_suspend = opteed_cpu_suspend_handler,
+	.svc_on_finish = opteed_cpu_on_finish_handler,
+	.svc_suspend_finish = opteed_cpu_suspend_finish_handler,
+	.svc_migrate = NULL,
+	.svc_migrate_info = opteed_cpu_migrate_info,
+	.svc_system_off = opteed_system_off,
+	.svc_system_reset = opteed_system_reset,
+};

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/opteed/opteed_private.h

@@ -0,0 +1,162 @@
+/*
+ * Copyright (c) 2013-2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef OPTEED_PRIVATE_H
+#define OPTEED_PRIVATE_H
+
+#include <platform_def.h>
+
+#include <arch.h>
+#include <bl31/interrupt_mgmt.h>
+#include <context.h>
+#include <lib/psci/psci.h>
+
+/*******************************************************************************
+ * OPTEE PM state information e.g. OPTEE is suspended, uninitialised etc
+ * and macros to access the state information in the per-cpu 'state' flags
+ ******************************************************************************/
+#define OPTEE_PSTATE_OFF		0
+#define OPTEE_PSTATE_ON			1
+#define OPTEE_PSTATE_SUSPEND		2
+#define OPTEE_PSTATE_SHIFT		0
+#define OPTEE_PSTATE_MASK		0x3
+#define get_optee_pstate(state)	((state >> OPTEE_PSTATE_SHIFT) & \
+				 OPTEE_PSTATE_MASK)
+#define clr_optee_pstate(state)	(state &= ~(OPTEE_PSTATE_MASK \
+					    << OPTEE_PSTATE_SHIFT))
+#define set_optee_pstate(st, pst) do {					       \
+					clr_optee_pstate(st);		       \
+					st |= (pst & OPTEE_PSTATE_MASK) <<     \
+						OPTEE_PSTATE_SHIFT;	       \
+				} while (0)
+
+
+/*******************************************************************************
+ * OPTEE execution state information i.e. aarch32 or aarch64
+ ******************************************************************************/
+#define OPTEE_AARCH32		MODE_RW_32
+#define OPTEE_AARCH64		MODE_RW_64
+
+/*******************************************************************************
+ * The OPTEED should know the type of OPTEE
+ ******************************************************************************/
+#define OPTEE_TYPE_UP		PSCI_TOS_NOT_UP_MIG_CAP
+#define OPTEE_TYPE_UPM		PSCI_TOS_UP_MIG_CAP
+#define OPTEE_TYPE_MP		PSCI_TOS_NOT_PRESENT_MP
+
+/*******************************************************************************
+ * OPTEE migrate type information as known to the OPTEED. We assume that
+ * the OPTEED is dealing with an MP Secure Payload.
+ ******************************************************************************/
+#define OPTEE_MIGRATE_INFO		OPTEE_TYPE_MP
+
+/*******************************************************************************
+ * Number of cpus that the present on this platform. TODO: Rely on a topology
+ * tree to determine this in the future to avoid assumptions about mpidr
+ * allocation
+ ******************************************************************************/
+#define OPTEED_CORE_COUNT		PLATFORM_CORE_COUNT
+
+/*******************************************************************************
+ * Constants that allow assembler code to preserve callee-saved registers of the
+ * C runtime context while performing a security state switch.
+ ******************************************************************************/
+#define OPTEED_C_RT_CTX_X19		0x0
+#define OPTEED_C_RT_CTX_X20		0x8
+#define OPTEED_C_RT_CTX_X21		0x10
+#define OPTEED_C_RT_CTX_X22		0x18
+#define OPTEED_C_RT_CTX_X23		0x20
+#define OPTEED_C_RT_CTX_X24		0x28
+#define OPTEED_C_RT_CTX_X25		0x30
+#define OPTEED_C_RT_CTX_X26		0x38
+#define OPTEED_C_RT_CTX_X27		0x40
+#define OPTEED_C_RT_CTX_X28		0x48
+#define OPTEED_C_RT_CTX_X29		0x50
+#define OPTEED_C_RT_CTX_X30		0x58
+#define OPTEED_C_RT_CTX_SIZE		0x60
+#define OPTEED_C_RT_CTX_ENTRIES		(OPTEED_C_RT_CTX_SIZE >> DWORD_SHIFT)
+
+#ifndef __ASSEMBLER__
+
+#include <stdint.h>
+
+#include <lib/cassert.h>
+
+typedef uint32_t optee_vector_isn_t;
+
+typedef struct optee_vectors {
+	optee_vector_isn_t yield_smc_entry;
+	optee_vector_isn_t fast_smc_entry;
+	optee_vector_isn_t cpu_on_entry;
+	optee_vector_isn_t cpu_off_entry;
+	optee_vector_isn_t cpu_resume_entry;
+	optee_vector_isn_t cpu_suspend_entry;
+	optee_vector_isn_t fiq_entry;
+	optee_vector_isn_t system_off_entry;
+	optee_vector_isn_t system_reset_entry;
+} optee_vectors_t;
+
+/*
+ * The number of arguments to save during a SMC call for OPTEE.
+ * Currently only x1 and x2 are used by OPTEE.
+ */
+#define OPTEE_NUM_ARGS	0x2
+
+/* AArch64 callee saved general purpose register context structure. */
+DEFINE_REG_STRUCT(c_rt_regs, OPTEED_C_RT_CTX_ENTRIES);
+
+/*
+ * Compile time assertion to ensure that both the compiler and linker
+ * have the same double word aligned view of the size of the C runtime
+ * register context.
+ */
+CASSERT(OPTEED_C_RT_CTX_SIZE == sizeof(c_rt_regs_t),	\
+	assert_spd_c_rt_regs_size_mismatch);
+
+/*******************************************************************************
+ * Structure which helps the OPTEED to maintain the per-cpu state of OPTEE.
+ * 'state'          - collection of flags to track OPTEE state e.g. on/off
+ * 'mpidr'          - mpidr to associate a context with a cpu
+ * 'c_rt_ctx'       - stack address to restore C runtime context from after
+ *                    returning from a synchronous entry into OPTEE.
+ * 'cpu_ctx'        - space to maintain OPTEE architectural state
+ ******************************************************************************/
+typedef struct optee_context {
+	uint32_t state;
+	uint64_t mpidr;
+	uint64_t c_rt_ctx;
+	cpu_context_t cpu_ctx;
+} optee_context_t;
+
+/* OPTEED power management handlers */
+extern const spd_pm_ops_t opteed_pm;
+
+/*******************************************************************************
+ * Forward declarations
+ ******************************************************************************/
+struct optee_vectors;
+
+/*******************************************************************************
+ * Function & Data prototypes
+ ******************************************************************************/
+uint64_t opteed_enter_sp(uint64_t *c_rt_ctx);
+void __dead2 opteed_exit_sp(uint64_t c_rt_ctx, uint64_t ret);
+uint64_t opteed_synchronous_sp_entry(optee_context_t *optee_ctx);
+void __dead2 opteed_synchronous_sp_exit(optee_context_t *optee_ctx, uint64_t ret);
+void opteed_init_optee_ep_state(struct entry_point_info *optee_entry_point,
+				uint32_t rw,
+				uint64_t pc,
+				uint64_t pageable_part,
+				uint64_t mem_limit,
+				uint64_t dt_addr,
+				optee_context_t *optee_ctx);
+
+extern optee_context_t opteed_sp_context[OPTEED_CORE_COUNT];
+extern uint32_t opteed_rw;
+extern struct optee_vectors *optee_vector_table;
+#endif /*__ASSEMBLER__*/
+
+#endif /* OPTEED_PRIVATE_H */

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/opteed/teesmc_opteed.h

@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) 2014-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/* Copyright (c) 2014, Linaro Limited. All rights reserved. */
+
+#ifndef TEESMC_OPTEED_H
+#define TEESMC_OPTEED_H
+
+/*
+ * This file specifies SMC function IDs used when returning from TEE to the
+ * secure monitor.
+ *
+ * All SMC Function IDs indicates SMC32 Calling Convention but will carry
+ * full 64 bit values in the argument registers if invoked from Aarch64
+ * mode. This violates the SMC Calling Convention, but since this
+ * convention only coveres API towards Normal World it's something that
+ * only concerns the OP-TEE Dispatcher in Trusted Firmware-A and OP-TEE
+ * OS at Secure EL1.
+ */
+
+/*
+ * Issued when returning from initial entry.
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_ENTRY_DONE
+ * r1/x1	Pointer to entry vector
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_ENTRY_DONE		0
+#define TEESMC_OPTEED_RETURN_ENTRY_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_ENTRY_DONE)
+
+
+
+/*
+ * Issued when returning from "cpu_on" vector
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_ON_DONE
+ * r1/x1	0 on success and anything else to indicate error condition
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_ON_DONE		1
+#define TEESMC_OPTEED_RETURN_ON_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_ON_DONE)
+
+/*
+ * Issued when returning from "cpu_off" vector
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_OFF_DONE
+ * r1/x1	0 on success and anything else to indicate error condition
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_OFF_DONE		2
+#define TEESMC_OPTEED_RETURN_OFF_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_OFF_DONE)
+
+/*
+ * Issued when returning from "cpu_suspend" vector
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_SUSPEND_DONE
+ * r1/x1	0 on success and anything else to indicate error condition
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_SUSPEND_DONE	3
+#define TEESMC_OPTEED_RETURN_SUSPEND_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_SUSPEND_DONE)
+
+/*
+ * Issued when returning from "cpu_resume" vector
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_RESUME_DONE
+ * r1/x1	0 on success and anything else to indicate error condition
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_RESUME_DONE		4
+#define TEESMC_OPTEED_RETURN_RESUME_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_RESUME_DONE)
+
+/*
+ * Issued when returning from "std_smc" or "fast_smc" vector
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_CALL_DONE
+ * r1-4/x1-4	Return value 0-3 which will passed to normal world in
+ *		r0-3/x0-3
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_CALL_DONE		5
+#define TEESMC_OPTEED_RETURN_CALL_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_CALL_DONE)
+
+/*
+ * Issued when returning from "fiq" vector
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_FIQ_DONE
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_FIQ_DONE		6
+#define TEESMC_OPTEED_RETURN_FIQ_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_FIQ_DONE)
+
+/*
+ * Issued when returning from "system_off" vector
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_SYSTEM_OFF_DONE
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_SYSTEM_OFF_DONE	7
+#define TEESMC_OPTEED_RETURN_SYSTEM_OFF_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_SYSTEM_OFF_DONE)
+
+/*
+ * Issued when returning from "system_reset" vector
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_SYSTEM_RESET_DONE
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_SYSTEM_RESET_DONE	8
+#define TEESMC_OPTEED_RETURN_SYSTEM_RESET_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_SYSTEM_RESET_DONE)
+
+#endif /*TEESMC_OPTEED_H*/

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/opteed/teesmc_opteed_macros.h

@@ -0,0 +1,17 @@
+/*
+ * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef TEESMC_OPTEED_MACROS_H
+#define TEESMC_OPTEED_MACROS_H
+
+#include <common/runtime_svc.h>
+
+#define TEESMC_OPTEED_RV(func_num) \
+		((SMC_TYPE_FAST << FUNCID_TYPE_SHIFT) | \
+		 ((SMC_32) << FUNCID_CC_SHIFT) | \
+		 (62 << FUNCID_OEN_SHIFT) | \
+		 ((func_num) & FUNCID_NUM_MASK))
+
+#endif /* TEESMC_OPTEED_MACROS_H */

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/pncd/pncd.mk

@@ -0,0 +1,24 @@
+# Copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+PNCD_DIR		:=	services/spd/pncd
+SPD_INCLUDES		:=	-Iinclude/bl32/pnc
+SPD_INCLUDES		+=	-Iinclude/common/
+
+SPD_SOURCES		:=	services/spd/pncd/pncd_common.c		\
+				services/spd/pncd/pncd_helpers.S	\
+				services/spd/pncd/pncd_main.c
+
+NEED_BL32		:=	yes
+
+# The following constants need to be defined:
+#   - SPD_PNCD_NS_IRQ: IRQ number used to notify NS world when SMC_ACTION_FROM_S is received
+#   - SPD_PNCD_S_IRQ: IRQ number used to notify S world when SMC_ACTION_FROM_NS is received
+$(eval $(call assert_numerics, SPD_PNCD_NS_IRQ SPD_PNCD_S_IRQ))
+
+$(eval $(call add_defines,\
+    $(sort \
+        SPD_PNCD_NS_IRQ \
+        SPD_PNCD_S_IRQ \
+)))

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/pncd/pncd_common.c

@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+
+#include "pncd_private.h"
+
+/*******************************************************************************
+ * Given a secure payload entrypoint info pointer, entry point PC & pointer to a
+ * context data structure, this function will initialize pnc context and entry
+ * point info for the secure payload
+ ******************************************************************************/
+void pncd_init_pnc_ep_state(struct entry_point_info *pnc_entry_point,
+				uint64_t pc,
+				pnc_context_t *pnc_ctx)
+{
+	uint32_t ep_attr;
+
+	/* Passing a NULL context is a critical programming error */
+	assert(pnc_ctx);
+	assert(pnc_entry_point);
+	assert(pc);
+
+	/* Associate this context with the current cpu */
+	pnc_ctx->mpidr = read_mpidr();
+
+	cm_set_context(&pnc_ctx->cpu_ctx, SECURE);
+
+	/* initialise an entrypoint to set up the CPU context */
+	ep_attr = SECURE | EP_ST_ENABLE;
+	if (read_sctlr_el3() & SCTLR_EE_BIT) {
+		ep_attr |= EP_EE_BIG;
+	}
+	SET_PARAM_HEAD(pnc_entry_point, PARAM_EP, VERSION_1, ep_attr);
+
+	pnc_entry_point->pc = pc;
+	pnc_entry_point->spsr = SPSR_64(MODE_EL1,
+					MODE_SP_ELX,
+					DISABLE_ALL_EXCEPTIONS);
+	memset(&pnc_entry_point->args, 0, sizeof(pnc_entry_point->args));
+}
+
+/*******************************************************************************
+ * This function takes an SP context pointer and:
+ * 1. Applies the S-EL1 system register context from pnc_ctx->cpu_ctx.
+ * 2. Saves the current C runtime state (callee saved registers) on the stack
+ *    frame and saves a reference to this state.
+ * 3. Calls el3_exit() so that the EL3 system and general purpose registers
+ *    from the pnc_ctx->cpu_ctx are used to enter the secure payload image.
+ ******************************************************************************/
+uint64_t pncd_synchronous_sp_entry(pnc_context_t *pnc_ctx)
+{
+	assert(pnc_ctx != NULL);
+	assert(pnc_ctx->c_rt_ctx == 0U);
+
+	/* Apply the Secure EL1 system register context and switch to it */
+	assert(cm_get_context(SECURE) == &pnc_ctx->cpu_ctx);
+	cm_el1_sysregs_context_restore(SECURE);
+#if CTX_INCLUDE_FPREGS
+	fpregs_context_restore(get_fpregs_ctx(cm_get_context(SECURE)));
+#endif
+	cm_set_next_eret_context(SECURE);
+
+	return pncd_enter_sp(&pnc_ctx->c_rt_ctx);
+}
+
+
+/*******************************************************************************
+ * This function takes an SP context pointer and:
+ * 1. Saves the S-EL1 system register context tp pnc_ctx->cpu_ctx.
+ * 2. Restores the current C runtime state (callee saved registers) from the
+ *    stack frame using the reference to this state saved in pncd_enter_sp().
+ * 3. It does not need to save any general purpose or EL3 system register state
+ *    as the generic smc entry routine should have saved those.
+ ******************************************************************************/
+void pncd_synchronous_sp_exit(pnc_context_t *pnc_ctx, uint64_t ret)
+{
+	assert(pnc_ctx != NULL);
+	/* Save the Secure EL1 system register context */
+	assert(cm_get_context(SECURE) == &pnc_ctx->cpu_ctx);
+	cm_el1_sysregs_context_save(SECURE);
+#if CTX_INCLUDE_FPREGS
+	fpregs_context_save(get_fpregs_ctx(cm_get_context(SECURE)));
+#endif
+
+	assert(pnc_ctx->c_rt_ctx != 0);
+	pncd_exit_sp(pnc_ctx->c_rt_ctx, ret);
+
+	/* Should never reach here */
+	panic();
+}

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/pncd/pncd_helpers.S

@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+#include "pncd_private.h"
+
+	.global	pncd_enter_sp
+	/* ---------------------------------------------
+	 * This function is called with SP_EL0 as stack.
+	 * Here we stash our EL3 callee-saved registers
+	 * on to the stack as a part of saving the C
+	 * runtime and enter the secure payload.
+	 * 'x0' contains a pointer to the memory where
+	 * the address of the C runtime context is to be
+	 * saved.
+	 * ---------------------------------------------
+	 */
+func pncd_enter_sp
+	/* Make space for the registers that we're going to save */
+	mov	x3, sp
+	str	x3, [x0, #0]
+	sub	sp, sp, #PNCD_C_RT_CTX_SIZE
+
+	/* Save callee-saved registers on to the stack */
+	stp	x19, x20, [sp, #PNCD_C_RT_CTX_X19]
+	stp	x21, x22, [sp, #PNCD_C_RT_CTX_X21]
+	stp	x23, x24, [sp, #PNCD_C_RT_CTX_X23]
+	stp	x25, x26, [sp, #PNCD_C_RT_CTX_X25]
+	stp	x27, x28, [sp, #PNCD_C_RT_CTX_X27]
+	stp	x29, x30, [sp, #PNCD_C_RT_CTX_X29]
+
+	/* ---------------------------------------------
+	 * Everything is setup now. el3_exit() will
+	 * use the secure context to restore to the
+	 * general purpose and EL3 system registers to
+	 * ERET into the secure payload.
+	 * ---------------------------------------------
+	 */
+	b	el3_exit
+endfunc pncd_enter_sp
+
+	/* ---------------------------------------------
+	 * This function is called 'x0' pointing to a C
+	 * runtime context saved in pncd_enter_sp(). It
+	 * restores the saved registers and jumps to
+	 * that runtime with 'x0' as the new sp. This
+	 * destroys the C runtime context that had been
+	 * built on the stack below the saved context by
+	 * the caller. Later the second parameter 'x1'
+	 * is passed as return value to the caller
+	 * ---------------------------------------------
+	 */
+	.global pncd_exit_sp
+func pncd_exit_sp
+	/* Restore the previous stack */
+	mov	sp, x0
+
+	/* Restore callee-saved registers on to the stack */
+	ldp	x19, x20, [x0, #(PNCD_C_RT_CTX_X19 - PNCD_C_RT_CTX_SIZE)]
+	ldp	x21, x22, [x0, #(PNCD_C_RT_CTX_X21 - PNCD_C_RT_CTX_SIZE)]
+	ldp	x23, x24, [x0, #(PNCD_C_RT_CTX_X23 - PNCD_C_RT_CTX_SIZE)]
+	ldp	x25, x26, [x0, #(PNCD_C_RT_CTX_X25 - PNCD_C_RT_CTX_SIZE)]
+	ldp	x27, x28, [x0, #(PNCD_C_RT_CTX_X27 - PNCD_C_RT_CTX_SIZE)]
+	ldp	x29, x30, [x0, #(PNCD_C_RT_CTX_X29 - PNCD_C_RT_CTX_SIZE)]
+
+	/* ---------------------------------------------
+	 * This should take us back to the instruction
+	 * after the call to the last pncd_enter_sp().
+	 * Place the second parameter to x0 so that the
+	 * caller will see it as a return value from the
+	 * original entry call
+	 * ---------------------------------------------
+	 */
+	mov	x0, x1
+	ret
+endfunc pncd_exit_sp

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/pncd/pncd_main.c

@@ -0,0 +1,471 @@
+/*
+ * Copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*******************************************************************************
+ * This is the Secure Payload Dispatcher (SPD). The dispatcher is meant to be a
+ * plug-in component to the Secure Monitor, registered as a runtime service. The
+ * SPD is expected to be a functional extension of the Secure Payload (SP) that
+ * executes in Secure EL1. The Secure Monitor will delegate all SMCs targeting
+ * the Trusted OS/Applications range to the dispatcher. The SPD will either
+ * handle the request locally or delegate it to the Secure Payload. It is also
+ * responsible for initialising and maintaining communication with the SP.
+ ******************************************************************************/
+
+#include <assert.h>
+#include <errno.h>
+#include <stddef.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <bl31/bl31.h>
+#include <bl31/interrupt_mgmt.h>
+#include <bl_common.h>
+#include <common/debug.h>
+#include <common/ep_info.h>
+#include <drivers/arm/gic_common.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/spinlock.h>
+#include <plat/common/platform.h>
+#include <pnc.h>
+#include "pncd_private.h"
+#include <runtime_svc.h>
+#include <tools_share/uuid.h>
+
+/*******************************************************************************
+ * Structure to keep track of ProvenCore state
+ ******************************************************************************/
+static pnc_context_t pncd_sp_context;
+
+static bool ree_info;
+static uint64_t ree_base_addr;
+static uint64_t ree_length;
+static uint64_t ree_tag;
+
+static bool pnc_initialized;
+
+static spinlock_t smc_handler_lock;
+
+static int pncd_init(void);
+
+static void context_save(unsigned long security_state)
+{
+	assert(sec_state_is_valid(security_state));
+
+	cm_el1_sysregs_context_save((uint32_t) security_state);
+#if CTX_INCLUDE_FPREGS
+	fpregs_context_save(get_fpregs_ctx(cm_get_context(security_state)));
+#endif
+}
+
+static void *context_restore(unsigned long security_state)
+{
+	void *handle;
+
+	assert(sec_state_is_valid(security_state));
+
+	/* Get a reference to the next context */
+	handle = cm_get_context((uint32_t) security_state);
+	assert(handle);
+
+	/* Restore state */
+	cm_el1_sysregs_context_restore((uint32_t) security_state);
+#if CTX_INCLUDE_FPREGS
+	fpregs_context_restore(get_fpregs_ctx(cm_get_context(security_state)));
+#endif
+
+	cm_set_next_eret_context((uint32_t) security_state);
+
+	return handle;
+}
+
+static uint64_t pncd_sel1_interrupt_handler(uint32_t id,
+		uint32_t flags, void *handle, void *cookie);
+
+/*******************************************************************************
+ * Switch context to the specified security state and return the targeted
+ * handle. Note that the context may remain unchanged if the switch is not
+ * allowed.
+ ******************************************************************************/
+void *pncd_context_switch_to(unsigned long security_state)
+{
+	unsigned long sec_state_from =
+	    security_state == SECURE ? NON_SECURE : SECURE;
+
+	assert(sec_state_is_valid(security_state));
+
+	/* Check if this is the first world switch */
+	if (!pnc_initialized) {
+		int rc;
+		uint32_t flags;
+
+		assert(sec_state_from == SECURE);
+
+		INFO("PnC initialization done\n");
+
+		/*
+		 * Register an interrupt handler for S-EL1 interrupts
+		 * when generated during code executing in the
+		 * non-secure state.
+		 */
+		flags = 0U;
+		set_interrupt_rm_flag(flags, NON_SECURE);
+		rc = register_interrupt_type_handler(INTR_TYPE_S_EL1,
+				pncd_sel1_interrupt_handler,
+				flags);
+		if (rc != 0) {
+			ERROR("Failed to register S-EL1 interrupt handler (%d)\n",
+			      rc);
+			panic();
+		}
+
+		context_save(SECURE);
+
+		pnc_initialized = true;
+
+		/*
+		 * Release the lock before restoring the EL3 context to
+		 * bl31_main.
+		 */
+		spin_unlock(&smc_handler_lock);
+
+		/*
+		 * SP reports completion. The SPD must have initiated
+		 * the original request through a synchronous entry
+		 * into the SP. Jump back to the original C runtime
+		 * context.
+		 */
+		pncd_synchronous_sp_exit(&pncd_sp_context, (uint64_t) 0x0);
+
+		/* Unreachable */
+		ERROR("Returned from pncd_synchronous_sp_exit... Should not happen\n");
+		panic();
+	}
+
+	/* Check that the world switch is allowed */
+	if (read_mpidr() != pncd_sp_context.mpidr) {
+		if (sec_state_from == SECURE) {
+			/*
+			 * Secure -> Non-Secure world switch initiated on a CPU where there
+			 * should be no Trusted OS running
+			 */
+			WARN("Secure to Non-Secure switch requested on CPU where ProvenCore is not supposed to be running...\n");
+		}
+
+		/*
+		 * Secure or Non-Secure world wants to switch world but there is no Secure
+		 * software on this core
+		 */
+		return cm_get_context((uint32_t) sec_state_from);
+	}
+
+	context_save(sec_state_from);
+
+	return context_restore(security_state);
+}
+
+/*******************************************************************************
+ * This function is the handler registered for S-EL1 interrupts by the PNCD. It
+ * validates the interrupt and upon success arranges entry into the PNC at
+ * 'pnc_sel1_intr_entry()' for handling the interrupt.
+ ******************************************************************************/
+static uint64_t pncd_sel1_interrupt_handler(uint32_t id,
+		uint32_t flags,
+		void *handle,
+		void *cookie)
+{
+	/* Check the security state when the exception was generated */
+	assert(get_interrupt_src_ss(flags) == NON_SECURE);
+
+	/* Sanity check the pointer to this cpu's context */
+	assert(handle == cm_get_context(NON_SECURE));
+
+	/* switch to PnC */
+	handle = pncd_context_switch_to(SECURE);
+
+	assert(handle != NULL);
+
+	SMC_RET0(handle);
+}
+
+#pragma weak plat_pncd_setup
+int plat_pncd_setup(void)
+{
+	return 0;
+}
+
+/*******************************************************************************
+ * Secure Payload Dispatcher setup. The SPD finds out the SP entrypoint and type
+ * (aarch32/aarch64) if not already known and initialises the context for entry
+ * into the SP for its initialisation.
+ ******************************************************************************/
+static int pncd_setup(void)
+{
+	entry_point_info_t *pnc_ep_info;
+
+	/*
+	 * Get information about the Secure Payload (BL32) image. Its
+	 * absence is a critical failure.
+	 *
+	 * TODO: Add support to conditionally include the SPD service
+	 */
+	pnc_ep_info = bl31_plat_get_next_image_ep_info(SECURE);
+	if (!pnc_ep_info) {
+		WARN("No PNC provided by BL2 boot loader, Booting device without PNC initialization. SMC`s destined for PNC will return SMC_UNK\n");
+		return 1;
+	}
+
+	/*
+	 * If there's no valid entry point for SP, we return a non-zero value
+	 * signalling failure initializing the service. We bail out without
+	 * registering any handlers
+	 */
+	if (!pnc_ep_info->pc) {
+		return 1;
+	}
+
+	pncd_init_pnc_ep_state(pnc_ep_info,
+			pnc_ep_info->pc,
+			&pncd_sp_context);
+
+	/*
+	 * All PNCD initialization done. Now register our init function with
+	 * BL31 for deferred invocation
+	 */
+	bl31_register_bl32_init(&pncd_init);
+	bl31_set_next_image_type(NON_SECURE);
+
+	return plat_pncd_setup();
+}
+
+/*******************************************************************************
+ * This function passes control to the Secure Payload image (BL32) for the first
+ * time on the primary cpu after a cold boot. It assumes that a valid secure
+ * context has already been created by pncd_setup() which can be directly used.
+ * It also assumes that a valid non-secure context has been initialised by PSCI
+ * so it does not need to save and restore any non-secure state. This function
+ * performs a synchronous entry into the Secure payload. The SP passes control
+ * back to this routine through a SMC.
+ ******************************************************************************/
+static int32_t pncd_init(void)
+{
+	entry_point_info_t *pnc_entry_point;
+	uint64_t rc = 0;
+
+	/*
+	 * Get information about the Secure Payload (BL32) image. Its
+	 * absence is a critical failure.
+	 */
+	pnc_entry_point = bl31_plat_get_next_image_ep_info(SECURE);
+	assert(pnc_entry_point);
+
+	cm_init_my_context(pnc_entry_point);
+
+	/*
+	 * Arrange for an entry into the test secure payload. It will be
+	 * returned via PNC_ENTRY_DONE case
+	 */
+	rc = pncd_synchronous_sp_entry(&pncd_sp_context);
+
+	/*
+	 * If everything went well at this point, the return value should be 0.
+	 */
+	return rc == 0;
+}
+
+#pragma weak plat_pncd_smc_handler
+/*******************************************************************************
+ * This function is responsible for handling the platform-specific SMCs in the
+ * Trusted OS/App range as defined in the SMC Calling Convention Document.
+ ******************************************************************************/
+uintptr_t plat_pncd_smc_handler(uint32_t smc_fid,
+		u_register_t x1,
+		u_register_t x2,
+		u_register_t x3,
+		u_register_t x4,
+		void *cookie,
+		void *handle,
+		u_register_t flags)
+{
+	(void) smc_fid;
+	(void) x1;
+	(void) x2;
+	(void) x3;
+	(void) x4;
+	(void) cookie;
+	(void) flags;
+
+	SMC_RET1(handle, SMC_UNK);
+}
+
+/*******************************************************************************
+ * This function is responsible for handling all SMCs in the Trusted OS/App
+ * range as defined in the SMC Calling Convention Document. It is also
+ * responsible for communicating with the Secure payload to delegate work and
+ * return results back to the non-secure state. Lastly it will also return any
+ * information that the secure payload needs to do the work assigned to it.
+ *
+ * It should only be called with the smc_handler_lock held.
+ ******************************************************************************/
+static uintptr_t pncd_smc_handler_unsafe(uint32_t smc_fid,
+		u_register_t x1,
+		u_register_t x2,
+		u_register_t x3,
+		u_register_t x4,
+		void *cookie,
+		void *handle,
+		u_register_t flags)
+{
+	uint32_t ns;
+
+	/* Determine which security state this SMC originated from */
+	ns = is_caller_non_secure(flags);
+
+	assert(ns != 0 || read_mpidr() == pncd_sp_context.mpidr);
+
+	switch (smc_fid) {
+	case SMC_CONFIG_SHAREDMEM:
+		if (ree_info) {
+			/* Do not Yield */
+			SMC_RET0(handle);
+		}
+
+		/*
+		 * Fetch the physical base address (x1) and size (x2) of the
+		 * shared memory allocated by the Non-Secure world. This memory
+		 * will be used by PNC to communicate with the Non-Secure world.
+		 * Verifying the validity of these values is up to the Trusted
+		 * OS.
+		 */
+		ree_base_addr = x1 | (x2 << 32);
+		ree_length = x3;
+		ree_tag = x4;
+
+		INFO("IN SMC_CONFIG_SHAREDMEM: addr=%lx, length=%lx, tag=%lx\n",
+		     (unsigned long) ree_base_addr,
+		     (unsigned long) ree_length,
+		     (unsigned long) ree_tag);
+
+		if ((ree_base_addr % 0x200000) != 0) {
+			SMC_RET1(handle, SMC_UNK);
+		}
+
+		if ((ree_length % 0x200000) != 0) {
+			SMC_RET1(handle, SMC_UNK);
+		}
+
+		ree_info = true;
+
+		/* Do not Yield */
+		SMC_RET4(handle, 0, 0, 0, 0);
+
+		break;
+
+	case SMC_GET_SHAREDMEM:
+		if (ree_info) {
+			x1 = (1U << 16) | ree_tag;
+			x2 = ree_base_addr & 0xFFFFFFFF;
+			x3 = (ree_base_addr >> 32) & 0xFFFFFFFF;
+			x4 = ree_length & 0xFFFFFFFF;
+			SMC_RET4(handle, x1, x2, x3, x4);
+		} else {
+			SMC_RET4(handle, 0, 0, 0, 0);
+		}
+
+		break;
+
+	case SMC_ACTION_FROM_NS:
+		if (ns == 0) {
+			SMC_RET1(handle, SMC_UNK);
+		}
+
+		if (SPD_PNCD_S_IRQ < MIN_PPI_ID) {
+			plat_ic_raise_s_el1_sgi(SPD_PNCD_S_IRQ,
+						pncd_sp_context.mpidr);
+		} else {
+			plat_ic_set_interrupt_pending(SPD_PNCD_S_IRQ);
+		}
+
+		SMC_RET0(handle);
+
+		break;
+
+	case SMC_ACTION_FROM_S:
+		if (ns != 0) {
+			SMC_RET1(handle, SMC_UNK);
+		}
+
+		if (SPD_PNCD_NS_IRQ < MIN_PPI_ID) {
+			/*
+			 * NS SGI is sent to the same core as the one running
+			 * PNC
+			 */
+			plat_ic_raise_ns_sgi(SPD_PNCD_NS_IRQ, read_mpidr());
+		} else {
+			plat_ic_set_interrupt_pending(SPD_PNCD_NS_IRQ);
+		}
+
+		SMC_RET0(handle);
+
+		break;
+
+	case SMC_YIELD:
+		assert(handle == cm_get_context(ns != 0 ? NON_SECURE : SECURE));
+		handle = pncd_context_switch_to(ns != 0 ? SECURE : NON_SECURE);
+
+		assert(handle != NULL);
+
+		SMC_RET0(handle);
+
+		break;
+
+	default:
+		INFO("Unknown smc: %x\n", smc_fid);
+		break;
+	}
+
+	return plat_pncd_smc_handler(smc_fid, x1, x2, x3, x4,
+				     cookie, handle, flags);
+}
+
+static uintptr_t pncd_smc_handler(uint32_t smc_fid,
+		u_register_t x1,
+		u_register_t x2,
+		u_register_t x3,
+		u_register_t x4,
+		void *cookie,
+		void *handle,
+		u_register_t flags)
+{
+	uintptr_t ret;
+
+	/* SMC handling is serialized */
+	spin_lock(&smc_handler_lock);
+	ret = pncd_smc_handler_unsafe(smc_fid, x1, x2, x3, x4, cookie, handle,
+								  flags);
+	spin_unlock(&smc_handler_lock);
+
+	return ret;
+}
+
+/* Define a SPD runtime service descriptor for fast SMC calls */
+DECLARE_RT_SVC(
+	pncd_fast,
+	OEN_TOS_START,
+	OEN_TOS_END,
+	SMC_TYPE_FAST,
+	pncd_setup,
+	pncd_smc_handler
+);
+
+/* Define a SPD runtime service descriptor for standard SMC calls */
+DECLARE_RT_SVC(
+	pncd_std,
+	OEN_TOS_START,
+	OEN_TOS_END,
+	SMC_TYPE_YIELD,
+	NULL,
+	pncd_smc_handler
+);

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/pncd/pncd_private.h

@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2021-2022, ARM Limited and Contributors. All rights reserved.
+ * Portions copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef __PNCD_PRIVATE_H__
+#define __PNCD_PRIVATE_H__
+
+#ifndef __ASSEMBLER__
+#include <stdint.h>
+#endif /* __ASSEMBLER __ */
+
+#include <context.h>
+#ifndef __ASSEMBLER__
+#include <lib/cassert.h>
+#endif /* __ASSEMBLER __ */
+
+#include <platform_def.h>
+
+/*******************************************************************************
+ * Constants that allow assembler code to preserve callee-saved registers of the
+ * C runtime context while performing a security state switch.
+ ******************************************************************************/
+#define PNCD_C_RT_CTX_X19		U(0x0)
+#define PNCD_C_RT_CTX_X20		U(0x8)
+#define PNCD_C_RT_CTX_X21		U(0x10)
+#define PNCD_C_RT_CTX_X22		U(0x18)
+#define PNCD_C_RT_CTX_X23		U(0x20)
+#define PNCD_C_RT_CTX_X24		U(0x28)
+#define PNCD_C_RT_CTX_X25		U(0x30)
+#define PNCD_C_RT_CTX_X26		U(0x38)
+#define PNCD_C_RT_CTX_X27		U(0x40)
+#define PNCD_C_RT_CTX_X28		U(0x48)
+#define PNCD_C_RT_CTX_X29		U(0x50)
+#define PNCD_C_RT_CTX_X30		U(0x58)
+#define PNCD_C_RT_CTX_SIZE		U(0x60)
+#define PNCD_C_RT_CTX_ENTRIES		(PNCD_C_RT_CTX_SIZE >> DWORD_SHIFT)
+
+#ifndef __ASSEMBLER__
+
+/* AArch64 callee saved general purpose register context structure. */
+DEFINE_REG_STRUCT(c_rt_regs, PNCD_C_RT_CTX_ENTRIES);
+
+/*
+ * Compile time assertion to ensure that both the compiler and linker
+ * have the same double word aligned view of the size of the C runtime
+ * register context.
+ */
+CASSERT(sizeof(c_rt_regs_t) == PNCD_C_RT_CTX_SIZE,
+		assert_spd_c_rt_regs_size_mismatch);
+
+/*******************************************************************************
+ * Structure which helps the SPD to maintain the per-cpu state of the SP.
+ * 'mpidr'          - mpidr of the CPU running PNC
+ * 'c_rt_ctx'       - stack address to restore C runtime context from after
+ *                    returning from a synchronous entry into the SP.
+ * 'cpu_ctx'        - space to maintain SP architectural state
+ ******************************************************************************/
+typedef struct pnc_context {
+	uint64_t mpidr;
+	uint64_t c_rt_ctx;
+	cpu_context_t cpu_ctx;
+} pnc_context_t;
+
+/*******************************************************************************
+ * Function & Data prototypes
+ ******************************************************************************/
+uint64_t pncd_enter_sp(uint64_t *c_rt_ctx);
+void __dead2 pncd_exit_sp(uint64_t c_rt_ctx, uint64_t ret);
+uint64_t pncd_synchronous_sp_entry(pnc_context_t *pnc_ctx);
+void __dead2 pncd_synchronous_sp_exit(pnc_context_t *pnc_ctx, uint64_t ret);
+void pncd_init_pnc_ep_state(struct entry_point_info *pnc_ep,
+				uint64_t pc,
+				pnc_context_t *pnc_ctx);
+#endif /* __ASSEMBLER__ */
+
+#endif /* __PNCD_PRIVATE_H__ */

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/tlkd/tlkd.mk

@@ -0,0 +1,14 @@
+#
+# Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq (${ERROR_DEPRECATED},0)
+SPD_INCLUDES		:=	-Iinclude/bl32/payloads
+endif
+
+SPD_SOURCES		:=	services/spd/tlkd/tlkd_common.c		\
+				services/spd/tlkd/tlkd_helpers.S	\
+				services/spd/tlkd/tlkd_main.c		\
+				services/spd/tlkd/tlkd_pm.c

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/tlkd/tlkd_common.c

@@ -0,0 +1,165 @@
+/*
+ * Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <lib/el3_runtime/context_mgmt.h>
+
+#include "tlkd_private.h"
+
+#define AT_MASK		3
+
+/*******************************************************************************
+ * This function helps the SP to translate NS/S virtual addresses.
+ ******************************************************************************/
+uint64_t tlkd_va_translate(uintptr_t va, int type)
+{
+	uint64_t pa;
+
+	if (type & TLK_TRANSLATE_NS_VADDR) {
+
+		/* save secure context */
+		cm_el1_sysregs_context_save(SECURE);
+
+		/* restore non-secure context */
+		cm_el1_sysregs_context_restore(NON_SECURE);
+
+		/* switch NS bit to start using 64-bit, non-secure mappings */
+		write_scr(cm_get_scr_el3(NON_SECURE));
+		isb();
+	}
+
+	int at = type & AT_MASK;
+	switch (at) {
+	case 0:
+		AT(ats12e1r, va);
+		break;
+	case 1:
+		AT(ats12e1w, va);
+		break;
+	case 2:
+		AT(ats12e0r, va);
+		break;
+	case 3:
+		AT(ats12e0w, va);
+		break;
+	default:
+		assert(0); /* Unreachable */
+		break;
+	}
+
+	/* get the (NS/S) physical address */
+	isb();
+	pa = read_par_el1();
+
+	/* Restore secure state */
+	if (type & TLK_TRANSLATE_NS_VADDR) {
+
+		/* restore secure context */
+		cm_el1_sysregs_context_restore(SECURE);
+
+		/* switch NS bit to start using 32-bit, secure mappings */
+		write_scr(cm_get_scr_el3(SECURE));
+		isb();
+	}
+
+	return pa;
+}
+
+/*******************************************************************************
+ * Given a secure payload entrypoint, register width, cpu id & pointer to a
+ * context data structure, this function will create a secure context ready for
+ * programming an entry into the secure payload.
+ ******************************************************************************/
+void tlkd_init_tlk_ep_state(struct entry_point_info *tlk_entry_point,
+			    uint32_t rw,
+			    uint64_t pc,
+			    tlk_context_t *tlk_ctx)
+{
+	uint32_t ep_attr, spsr;
+
+	/* Passing a NULL context is a critical programming error */
+	assert(tlk_ctx);
+	assert(tlk_entry_point);
+	assert(pc);
+
+	/* Associate this context with the cpu specified */
+	tlk_ctx->mpidr = read_mpidr_el1();
+	clr_yield_smc_active_flag(tlk_ctx->state);
+	cm_set_context(&tlk_ctx->cpu_ctx, SECURE);
+
+	if (rw == SP_AARCH64)
+		spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+	else
+		spsr = SPSR_MODE32(MODE32_svc,
+				   SPSR_T_ARM,
+				   read_sctlr_el3() & SCTLR_EE_BIT,
+				   DISABLE_ALL_EXCEPTIONS);
+
+	/* initialise an entrypoint to set up the CPU context */
+	ep_attr = SECURE | EP_ST_ENABLE;
+	if (read_sctlr_el3() & SCTLR_EE_BIT)
+		ep_attr |= EP_EE_BIG;
+	SET_PARAM_HEAD(tlk_entry_point, PARAM_EP, VERSION_1, ep_attr);
+
+	tlk_entry_point->pc = pc;
+	tlk_entry_point->spsr = spsr;
+}
+
+/*******************************************************************************
+ * This function takes a TLK context pointer and:
+ * 1. Applies the S-EL1 system register context from tlk_ctx->cpu_ctx.
+ * 2. Saves the current C runtime state (callee saved registers) on the stack
+ *    frame and saves a reference to this state.
+ * 3. Calls el3_exit() so that the EL3 system and general purpose registers
+ *    from the tlk_ctx->cpu_ctx are used to enter the secure payload image.
+ ******************************************************************************/
+uint64_t tlkd_synchronous_sp_entry(tlk_context_t *tlk_ctx)
+{
+	uint64_t rc;
+
+	/* Passing a NULL context is a critical programming error */
+	assert(tlk_ctx);
+
+	/* Apply the Secure EL1 system register context and switch to it */
+	assert(cm_get_context(SECURE) == &tlk_ctx->cpu_ctx);
+	cm_el1_sysregs_context_restore(SECURE);
+	cm_set_next_eret_context(SECURE);
+
+	rc = tlkd_enter_sp(&tlk_ctx->c_rt_ctx);
+#if ENABLE_ASSERTIONS
+	tlk_ctx->c_rt_ctx = 0;
+#endif
+
+	return rc;
+}
+
+/*******************************************************************************
+ * This function takes a TLK context pointer and:
+ * 1. Saves the S-EL1 system register context to tlk_ctx->cpu_ctx.
+ * 2. Restores the current C runtime state (callee saved registers) from the
+ *    stack frame using reference to this state saved in tlkd_enter_sp().
+ * 3. It does not need to save any general purpose or EL3 system register state
+ *    as the generic smc entry routine should have saved those.
+ ******************************************************************************/
+void tlkd_synchronous_sp_exit(tlk_context_t *tlk_ctx, uint64_t ret)
+{
+	/* Passing a NULL context is a critical programming error */
+	assert(tlk_ctx);
+
+	/* Save the Secure EL1 system register context */
+	assert(cm_get_context(SECURE) == &tlk_ctx->cpu_ctx);
+	cm_el1_sysregs_context_save(SECURE);
+
+	assert(tlk_ctx->c_rt_ctx != 0);
+	tlkd_exit_sp(tlk_ctx->c_rt_ctx, ret);
+
+	/* Should never reach here */
+	assert(0);
+}

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/tlkd/tlkd_helpers.S

@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+#include "tlkd_private.h"
+
+	.global tlkd_enter_sp
+	.global tlkd_exit_sp
+
+	/* ---------------------------------------------
+	 * This function is called with SP_EL0 as stack.
+	 * Here we stash our EL3 callee-saved registers
+	 * on to the stack as a part of saving the C
+	 * runtime and enter the secure payload.
+	 * 'x0' contains a pointer to the memory where
+	 * the address of the C runtime context is to be
+	 * saved.
+	 * ---------------------------------------------
+	 */
+func tlkd_enter_sp
+	/* Make space for the registers that we're going to save */
+	mov	x3, sp
+	str	x3, [x0, #0]
+	sub	sp, sp, #TLKD_C_RT_CTX_SIZE
+
+	/* Save callee-saved registers on to the stack */
+	stp	x19, x20, [sp, #TLKD_C_RT_CTX_X19]
+	stp	x21, x22, [sp, #TLKD_C_RT_CTX_X21]
+	stp	x23, x24, [sp, #TLKD_C_RT_CTX_X23]
+	stp	x25, x26, [sp, #TLKD_C_RT_CTX_X25]
+	stp	x27, x28, [sp, #TLKD_C_RT_CTX_X27]
+	stp	x29, x30, [sp, #TLKD_C_RT_CTX_X29]
+
+	/* ----------------------------------------------
+	 * Everything is setup now. el3_exit() will
+	 * use the secure context to restore to the
+	 * general purpose and EL3 system registers to
+	 * ERET into the secure payload.
+	 * ----------------------------------------------
+	 */
+	b	el3_exit
+endfunc tlkd_enter_sp
+
+	/* ----------------------------------------------
+	 * This function is called with 'x0' pointing to
+	 * a C runtime context saved in tlkd_enter_sp().
+	 * It restores the saved registers and jumps to
+	 * that runtime with 'x0' as the new sp. This
+	 * destroys the C runtime context that had been
+	 * built on the stack below the saved context by
+	 * the caller. Later the second parameter 'x1'
+	 * is passed as return value to the caller
+	 * ----------------------------------------------
+	 */
+func tlkd_exit_sp
+	/* Restore the previous stack */
+	mov	sp, x0
+
+	/* Restore callee-saved registers on to the stack */
+	ldp	x19, x20, [x0, #(TLKD_C_RT_CTX_X19 - TLKD_C_RT_CTX_SIZE)]
+	ldp	x21, x22, [x0, #(TLKD_C_RT_CTX_X21 - TLKD_C_RT_CTX_SIZE)]
+	ldp	x23, x24, [x0, #(TLKD_C_RT_CTX_X23 - TLKD_C_RT_CTX_SIZE)]
+	ldp	x25, x26, [x0, #(TLKD_C_RT_CTX_X25 - TLKD_C_RT_CTX_SIZE)]
+	ldp	x27, x28, [x0, #(TLKD_C_RT_CTX_X27 - TLKD_C_RT_CTX_SIZE)]
+	ldp	x29, x30, [x0, #(TLKD_C_RT_CTX_X29 - TLKD_C_RT_CTX_SIZE)]
+
+	/* ------------------------------------------------
+	 * This should take us back to the instruction
+	 * after the call to the last tlkd_enter_sp().
+	 * Place the second parameter to x0 so that the
+	 * caller will see it as a return value from the
+	 * original entry call
+	 * ------------------------------------------------
+	 */
+	mov	x0, x1
+	ret
+endfunc tlkd_exit_sp

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/tlkd/tlkd_main.c

@@ -0,0 +1,546 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020, NVIDIA Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*******************************************************************************
+ * This is the Secure Payload Dispatcher (SPD). The dispatcher is meant to be a
+ * plug-in component to the Secure Monitor, registered as a runtime service. The
+ * SPD is expected to be a functional extension of the Secure Payload (SP) that
+ * executes in Secure EL1. The Secure Monitor will delegate all SMCs targeting
+ * the Trusted OS/Applications range to the dispatcher. The SPD will either
+ * handle the request locally or delegate it to the Secure Payload. It is also
+ * responsible for initialising and maintaining communication with the SP.
+ ******************************************************************************/
+#include <assert.h>
+#include <bl31/interrupt_mgmt.h>
+#include <errno.h>
+#include <stddef.h>
+
+#include <arch_helpers.h>
+#include <bl31/bl31.h>
+#include <bl32/payloads/tlk.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <plat/common/platform.h>
+#include <tools_share/uuid.h>
+
+#include "tlkd_private.h"
+
+extern const spd_pm_ops_t tlkd_pm_ops;
+
+/*******************************************************************************
+ * Per-cpu Secure Payload state
+ ******************************************************************************/
+tlk_context_t tlk_ctx;
+
+/*******************************************************************************
+ * CPU number on which TLK booted up
+ ******************************************************************************/
+static uint32_t boot_cpu;
+
+/* TLK UID: RFC-4122 compliant UUID (version-5, sha-1) */
+DEFINE_SVC_UUID2(tlk_uuid,
+	0xc9e911bd, 0xba2b, 0xee52, 0xb1, 0x72,
+	0x46, 0x1f, 0xba, 0x97, 0x7f, 0x63);
+
+static int32_t tlkd_init(void);
+
+/*******************************************************************************
+ * Secure Payload Dispatcher's timer interrupt handler
+ ******************************************************************************/
+static uint64_t tlkd_interrupt_handler(uint32_t id,
+					uint32_t flags,
+					void *handle,
+					void *cookie)
+{
+	cpu_context_t *s_cpu_context;
+	int irq = plat_ic_get_pending_interrupt_id();
+
+	/* acknowledge the interrupt and mark it complete */
+	(void)plat_ic_acknowledge_interrupt();
+	plat_ic_end_of_interrupt(irq);
+
+	/*
+	 * Disable the routing of NS interrupts from secure world to
+	 * EL3 while interrupted on this core.
+	 */
+	disable_intr_rm_local(INTR_TYPE_S_EL1, SECURE);
+
+	/* Check the security state when the exception was generated */
+	assert(get_interrupt_src_ss(flags) == NON_SECURE);
+	assert(handle == cm_get_context(NON_SECURE));
+
+	/* Save non-secure state */
+	cm_el1_sysregs_context_save(NON_SECURE);
+
+	/* Get a reference to the secure context */
+	s_cpu_context = cm_get_context(SECURE);
+	assert(s_cpu_context);
+
+	/*
+	 * Restore non-secure state. There is no need to save the
+	 * secure system register context since the SP was supposed
+	 * to preserve it during S-EL1 interrupt handling.
+	 */
+	cm_el1_sysregs_context_restore(SECURE);
+	cm_set_next_eret_context(SECURE);
+
+	/* Provide the IRQ number to the SPD */
+	SMC_RET4(s_cpu_context, (uint32_t)TLK_IRQ_FIRED, 0, (uint32_t)irq, 0);
+}
+
+/*******************************************************************************
+ * Secure Payload Dispatcher setup. The SPD finds out the SP entrypoint and type
+ * (aarch32/aarch64) if not already known and initialises the context for entry
+ * into the SP for its initialisation.
+ ******************************************************************************/
+static int32_t tlkd_setup(void)
+{
+	entry_point_info_t *tlk_ep_info;
+	uint32_t flags;
+	int32_t ret;
+
+	/*
+	 * Get information about the Secure Payload (BL32) image. Its
+	 * absence is a critical failure.
+	 */
+	tlk_ep_info = bl31_plat_get_next_image_ep_info(SECURE);
+	if (!tlk_ep_info) {
+		WARN("No SP provided. Booting device without SP"
+			" initialization. SMC`s destined for SP"
+			" will return SMC_UNK\n");
+		return 1;
+	}
+
+	/*
+	 * If there's no valid entry point for SP, we return a non-zero value
+	 * signalling failure initializing the service. We bail out without
+	 * registering any handlers
+	 */
+	if (!tlk_ep_info->pc)
+		return 1;
+
+	/*
+	 * Inspect the SP image's SPSR and determine it's execution state
+	 * i.e whether AArch32 or AArch64.
+	 */
+	tlkd_init_tlk_ep_state(tlk_ep_info,
+		(tlk_ep_info->spsr >> MODE_RW_SHIFT) & MODE_RW_MASK,
+		tlk_ep_info->pc,
+		&tlk_ctx);
+
+	/* get a list of all S-EL1 IRQs from the platform */
+
+	/* register interrupt handler */
+	flags = 0;
+	set_interrupt_rm_flag(flags, NON_SECURE);
+	ret = register_interrupt_type_handler(INTR_TYPE_S_EL1,
+					      tlkd_interrupt_handler,
+					      flags);
+	if (ret != 0) {
+		ERROR("failed to register tlkd interrupt handler (%d)\n", ret);
+	}
+
+	/*
+	 * All TLK SPD initialization done. Now register our init function
+	 * with BL31 for deferred invocation
+	 */
+	bl31_register_bl32_init(&tlkd_init);
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function passes control to the Secure Payload image (BL32) for the first
+ * time on the primary cpu after a cold boot. It assumes that a valid secure
+ * context has already been created by tlkd_setup() which can be directly
+ * used. This function performs a synchronous entry into the Secure payload.
+ * The SP passes control back to this routine through a SMC.
+ ******************************************************************************/
+static int32_t tlkd_init(void)
+{
+	entry_point_info_t *tlk_entry_point;
+
+	/*
+	 * Get information about the Secure Payload (BL32) image. Its
+	 * absence is a critical failure.
+	 */
+	tlk_entry_point = bl31_plat_get_next_image_ep_info(SECURE);
+	assert(tlk_entry_point);
+
+	cm_init_my_context(tlk_entry_point);
+
+	/*
+	 * TLK runs only on a single CPU. Store the value of the boot
+	 * CPU for sanity checking later.
+	 */
+	boot_cpu = plat_my_core_pos();
+
+	/*
+	 * Arrange for an entry into the test secure payload.
+	 */
+	return tlkd_synchronous_sp_entry(&tlk_ctx);
+}
+
+/*******************************************************************************
+ * This function is responsible for handling all SMCs in the Trusted OS/App
+ * range from the non-secure state as defined in the SMC Calling Convention
+ * Document. It is also responsible for communicating with the Secure payload
+ * to delegate work and return results back to the non-secure state. Lastly it
+ * will also return any information that the secure payload needs to do the
+ * work assigned to it.
+ ******************************************************************************/
+static uintptr_t tlkd_smc_handler(uint32_t smc_fid,
+			 u_register_t x1,
+			 u_register_t x2,
+			 u_register_t x3,
+			 u_register_t x4,
+			 void *cookie,
+			 void *handle,
+			 u_register_t flags)
+{
+	cpu_context_t *ns_cpu_context;
+	gp_regs_t *gp_regs;
+	uint32_t ns;
+	uint64_t par;
+
+	/* Passing a NULL context is a critical programming error */
+	assert(handle);
+
+	/* These SMCs are only supported by a single CPU */
+	if (boot_cpu != plat_my_core_pos())
+		SMC_RET1(handle, SMC_UNK);
+
+	/* Determine which security state this SMC originated from */
+	ns = is_caller_non_secure(flags);
+
+	switch (smc_fid) {
+
+	/*
+	 * This function ID is used by SP to indicate that it was
+	 * preempted by a non-secure world IRQ.
+	 */
+	case TLK_PREEMPTED:
+
+		if (ns)
+			SMC_RET1(handle, SMC_UNK);
+
+		assert(handle == cm_get_context(SECURE));
+		cm_el1_sysregs_context_save(SECURE);
+
+		/* Get a reference to the non-secure context */
+		ns_cpu_context = cm_get_context(NON_SECURE);
+		assert(ns_cpu_context);
+
+		/*
+		 * Restore non-secure state. There is no need to save the
+		 * secure system register context since the SP was supposed
+		 * to preserve it during S-EL1 interrupt handling.
+		 */
+		cm_el1_sysregs_context_restore(NON_SECURE);
+		cm_set_next_eret_context(NON_SECURE);
+
+		SMC_RET1(ns_cpu_context, x1);
+
+	/*
+	 * This is a request from the non-secure context to:
+	 *
+	 * a. register shared memory with the SP for storing it's
+	 *    activity logs.
+	 * b. register shared memory with the SP for passing args
+	 *    required for maintaining sessions with the Trusted
+	 *    Applications.
+	 * c. register shared persistent buffers for secure storage
+	 * d. register NS DRAM ranges passed by Cboot
+	 * e. register Root of Trust parameters from Cboot for Verified Boot
+	 * f. open/close sessions
+	 * g. issue commands to the Trusted Apps
+	 * h. resume the preempted yielding SMC call.
+	 */
+	case TLK_REGISTER_LOGBUF:
+	case TLK_REGISTER_REQBUF:
+	case TLK_SS_REGISTER_HANDLER:
+	case TLK_REGISTER_NS_DRAM_RANGES:
+	case TLK_SET_ROOT_OF_TRUST:
+	case TLK_OPEN_TA_SESSION:
+	case TLK_CLOSE_TA_SESSION:
+	case TLK_TA_LAUNCH_OP:
+	case TLK_TA_SEND_EVENT:
+	case TLK_RESUME_FID:
+	case TLK_SET_BL_VERSION:
+	case TLK_LOCK_BL_INTERFACE:
+	case TLK_BL_RPMB_SERVICE:
+
+		if (!ns)
+			SMC_RET1(handle, SMC_UNK);
+
+		/*
+		 * This is a fresh request from the non-secure client.
+		 * The parameters are in x1 and x2. Figure out which
+		 * registers need to be preserved, save the non-secure
+		 * state and send the request to the secure payload.
+		 */
+		assert(handle == cm_get_context(NON_SECURE));
+
+		/*
+		 * Check if we are already processing a yielding SMC
+		 * call. Of all the supported fids, only the "resume"
+		 * fid expects the flag to be set.
+		 */
+		if (smc_fid == TLK_RESUME_FID) {
+			if (!get_yield_smc_active_flag(tlk_ctx.state))
+				SMC_RET1(handle, SMC_UNK);
+		} else {
+			if (get_yield_smc_active_flag(tlk_ctx.state))
+				SMC_RET1(handle, SMC_UNK);
+		}
+
+		cm_el1_sysregs_context_save(NON_SECURE);
+
+		/*
+		 * Verify if there is a valid context to use.
+		 */
+		assert(&tlk_ctx.cpu_ctx == cm_get_context(SECURE));
+
+		/*
+		 * Mark the SP state as active.
+		 */
+		set_yield_smc_active_flag(tlk_ctx.state);
+
+		/*
+		 * We are done stashing the non-secure context. Ask the
+		 * secure payload to do the work now.
+		 */
+		cm_el1_sysregs_context_restore(SECURE);
+		cm_set_next_eret_context(SECURE);
+
+		/*
+		 * TLK is a 32-bit Trusted OS and so expects the SMC
+		 * arguments via r0-r7. TLK expects the monitor frame
+		 * registers to be 64-bits long. Hence, we pass x0 in
+		 * r0-r1, x1 in r2-r3, x3 in r4-r5 and x4 in r6-r7.
+		 *
+		 * As smc_fid is a uint32 value, r1 contains 0.
+		 */
+		gp_regs = get_gpregs_ctx(&tlk_ctx.cpu_ctx);
+		write_ctx_reg(gp_regs, CTX_GPREG_X4, (uint32_t)x2);
+		write_ctx_reg(gp_regs, CTX_GPREG_X5, (uint32_t)(x2 >> 32));
+		write_ctx_reg(gp_regs, CTX_GPREG_X6, (uint32_t)x3);
+		write_ctx_reg(gp_regs, CTX_GPREG_X7, (uint32_t)(x3 >> 32));
+		SMC_RET4(&tlk_ctx.cpu_ctx, smc_fid, 0, (uint32_t)x1,
+			(uint32_t)(x1 >> 32));
+
+	/*
+	 * Translate NS/EL1-S virtual addresses.
+	 *
+	 * x1 = virtual address
+	 * x3 = type (NS/S)
+	 *
+	 * Returns PA:lo in r0, PA:hi in r1.
+	 */
+	case TLK_VA_TRANSLATE:
+
+		/* Should be invoked only by secure world */
+		if (ns)
+			SMC_RET1(handle, SMC_UNK);
+
+		/* NS virtual addresses are 64-bit long */
+		if (x3 & TLK_TRANSLATE_NS_VADDR)
+			x1 = (uint32_t)x1 | (x2 << 32);
+
+		if (!x1)
+			SMC_RET1(handle, SMC_UNK);
+
+		/*
+		 * TODO: Sanity check x1. This would require platform
+		 * support.
+		 */
+
+		/* virtual address and type: ns/s */
+		par = tlkd_va_translate(x1, x3);
+
+		/* return physical address in r0-r1 */
+		SMC_RET4(handle, (uint32_t)par, (uint32_t)(par >> 32), 0, 0);
+
+	/*
+	 * This is a request from the SP to mark completion of
+	 * a yielding function ID.
+	 */
+	case TLK_REQUEST_DONE:
+		if (ns)
+			SMC_RET1(handle, SMC_UNK);
+
+		/*
+		 * Mark the SP state as inactive.
+		 */
+		clr_yield_smc_active_flag(tlk_ctx.state);
+
+		/* Get a reference to the non-secure context */
+		ns_cpu_context = cm_get_context(NON_SECURE);
+		assert(ns_cpu_context);
+
+		/*
+		 * This is a request completion SMC and we must switch to
+		 * the non-secure world to pass the result.
+		 */
+		cm_el1_sysregs_context_save(SECURE);
+
+		/*
+		 * We are done stashing the secure context. Switch to the
+		 * non-secure context and return the result.
+		 */
+		cm_el1_sysregs_context_restore(NON_SECURE);
+		cm_set_next_eret_context(NON_SECURE);
+		SMC_RET1(ns_cpu_context, x1);
+
+	/*
+	 * This function ID is used only by the SP to indicate it has
+	 * finished initialising itself after a cold boot
+	 */
+	case TLK_ENTRY_DONE:
+		if (ns)
+			SMC_RET1(handle, SMC_UNK);
+
+		/*
+		 * SP has been successfully initialized. Register power
+		 * management hooks with PSCI
+		 */
+		psci_register_spd_pm_hook(&tlkd_pm_ops);
+
+		/*
+		 * TLK reports completion. The SPD must have initiated
+		 * the original request through a synchronous entry
+		 * into the SP. Jump back to the original C runtime
+		 * context.
+		 */
+		tlkd_synchronous_sp_exit(&tlk_ctx, x1);
+		break;
+
+	/*
+	 * These function IDs are used only by TLK to indicate it has
+	 * finished:
+	 * 1. suspending itself after an earlier psci cpu_suspend
+	 *    request.
+	 * 2. resuming itself after an earlier psci cpu_suspend
+	 *    request.
+	 * 3. powering down after an earlier psci system_off/system_reset
+	 *    request.
+	 */
+	case TLK_SUSPEND_DONE:
+	case TLK_RESUME_DONE:
+
+		if (ns)
+			SMC_RET1(handle, SMC_UNK);
+
+		/*
+		 * TLK reports completion. TLKD must have initiated the
+		 * original request through a synchronous entry into the SP.
+		 * Jump back to the original C runtime context, and pass x1 as
+		 * return value to the caller
+		 */
+		tlkd_synchronous_sp_exit(&tlk_ctx, x1);
+		break;
+
+	/*
+	 * This function ID is used by SP to indicate that it has completed
+	 * handling the secure interrupt.
+	 */
+	case TLK_IRQ_DONE:
+
+		if (ns)
+			SMC_RET1(handle, SMC_UNK);
+
+		assert(handle == cm_get_context(SECURE));
+
+		/* save secure world context */
+		cm_el1_sysregs_context_save(SECURE);
+
+		/* Get a reference to the non-secure context */
+		ns_cpu_context = cm_get_context(NON_SECURE);
+		assert(ns_cpu_context);
+
+		/*
+		 * Restore non-secure state. There is no need to save the
+		 * secure system register context since the SP was supposed
+		 * to preserve it during S-EL1 interrupt handling.
+		 */
+		cm_el1_sysregs_context_restore(NON_SECURE);
+		cm_set_next_eret_context(NON_SECURE);
+
+		SMC_RET0(ns_cpu_context);
+
+	/*
+	 * Return the number of service function IDs implemented to
+	 * provide service to non-secure
+	 */
+	case TOS_CALL_COUNT:
+		SMC_RET1(handle, TLK_NUM_FID);
+
+	/*
+	 * Return TLK's UID to the caller
+	 */
+	case TOS_UID:
+		SMC_UUID_RET(handle, tlk_uuid);
+
+	/*
+	 * Return the version of current implementation
+	 */
+	case TOS_CALL_VERSION:
+		SMC_RET2(handle, TLK_VERSION_MAJOR, TLK_VERSION_MINOR);
+
+	default:
+		WARN("%s: Unhandled SMC: 0x%x\n", __func__, smc_fid);
+		break;
+	}
+
+	SMC_RET1(handle, SMC_UNK);
+}
+
+/* Define a SPD runtime service descriptor for fast SMC calls */
+DECLARE_RT_SVC(
+	tlkd_tos_fast,
+
+	OEN_TOS_START,
+	OEN_TOS_END,
+	SMC_TYPE_FAST,
+	tlkd_setup,
+	tlkd_smc_handler
+);
+
+/* Define a SPD runtime service descriptor for yielding SMC calls */
+DECLARE_RT_SVC(
+	tlkd_tos_std,
+
+	OEN_TOS_START,
+	OEN_TOS_END,
+	SMC_TYPE_YIELD,
+	NULL,
+	tlkd_smc_handler
+);
+
+/* Define a SPD runtime service descriptor for fast SMC calls */
+DECLARE_RT_SVC(
+	tlkd_tap_fast,
+
+	OEN_TAP_START,
+	OEN_TAP_END,
+	SMC_TYPE_FAST,
+	NULL,
+	tlkd_smc_handler
+);
+
+/* Define a SPD runtime service descriptor for yielding SMC calls */
+DECLARE_RT_SVC(
+	tlkd_tap_std,
+
+	OEN_TAP_START,
+	OEN_TAP_END,
+	SMC_TYPE_YIELD,
+	NULL,
+	tlkd_smc_handler
+);

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/tlkd/tlkd_pm.c

@@ -0,0 +1,109 @@
+/*
+ * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020, NVIDIA Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <bl32/payloads/tlk.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/psci/psci.h>
+
+#include "tlkd_private.h"
+
+extern tlk_context_t tlk_ctx;
+
+#define MPIDR_CPU0	0x80000000
+
+/*******************************************************************************
+ * Return the type of payload TLKD is dealing with. Report the current
+ * resident cpu (mpidr format) if it is a UP/UP migratable payload.
+ ******************************************************************************/
+static int32_t cpu_migrate_info(u_register_t *resident_cpu)
+{
+	/* the payload runs only on CPU0 */
+	*resident_cpu = MPIDR_CPU0;
+
+	/* Uniprocessor, not migrate capable payload */
+	return PSCI_TOS_NOT_UP_MIG_CAP;
+}
+
+/*******************************************************************************
+ * This cpu is being suspended. Inform TLK of the SYSTEM_SUSPEND event, so
+ * that it can pass this information to its Trusted Apps.
+ ******************************************************************************/
+static void cpu_suspend_handler(u_register_t suspend_level)
+{
+	gp_regs_t *gp_regs;
+	int cpu = read_mpidr() & MPIDR_CPU_MASK;
+	int32_t rc = 0;
+
+	/*
+	 * TLK runs only on CPU0 and suspends its Trusted Apps during
+	 * SYSTEM_SUSPEND. It has no role to play during CPU_SUSPEND.
+	 */
+	if ((cpu != 0) || (suspend_level != PLAT_MAX_PWR_LVL))
+		return;
+
+	/* pass system suspend event to TLK */
+	gp_regs = get_gpregs_ctx(&tlk_ctx.cpu_ctx);
+	write_ctx_reg(gp_regs, CTX_GPREG_X0, TLK_SYSTEM_SUSPEND);
+
+	/* Program the entry point and enter TLK */
+	rc = tlkd_synchronous_sp_entry(&tlk_ctx);
+
+	/*
+	 * Read the response from TLK. A non-zero return means that
+	 * something went wrong while communicating with it.
+	 */
+	if (rc != 0)
+		panic();
+}
+
+/*******************************************************************************
+ * This cpu is being resumed. Inform TLK of the SYSTEM_SUSPEND exit, so
+ * that it can pass this information to its Trusted Apps.
+ ******************************************************************************/
+static void cpu_resume_handler(u_register_t suspend_level)
+{
+	gp_regs_t *gp_regs;
+	int cpu = read_mpidr() & MPIDR_CPU_MASK;
+	int32_t rc = 0;
+
+	/*
+	 * TLK runs only on CPU0 and resumes its Trusted Apps during
+	 * SYSTEM_SUSPEND exit. It has no role to play during CPU_SUSPEND
+	 * exit.
+	 */
+	if ((cpu != 0) || (suspend_level != PLAT_MAX_PWR_LVL))
+		return;
+
+	/* pass system resume event to TLK */
+	gp_regs = get_gpregs_ctx(&tlk_ctx.cpu_ctx);
+	write_ctx_reg(gp_regs, CTX_GPREG_X0, TLK_SYSTEM_RESUME);
+
+	/* Program the entry point and enter TLK */
+	rc = tlkd_synchronous_sp_entry(&tlk_ctx);
+
+	/*
+	 * Read the response from TLK. A non-zero return means that
+	 * something went wrong while communicating with it.
+	 */
+	if (rc != 0)
+		panic();
+}
+
+/*******************************************************************************
+ * Structure populated by the Dispatcher to be given a chance to perform any
+ * bookkeeping before PSCI executes a power mgmt.  operation.
+ ******************************************************************************/
+const spd_pm_ops_t tlkd_pm_ops = {
+	.svc_migrate_info = cpu_migrate_info,
+	.svc_suspend = cpu_suspend_handler,
+	.svc_suspend_finish = cpu_resume_handler,
+};

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/tlkd/tlkd_private.h

@@ -0,0 +1,124 @@
+/*
+ * Copyright (c) 2015-2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef TLKD_PRIVATE_H
+#define TLKD_PRIVATE_H
+
+#include <platform_def.h>
+
+#include <arch.h>
+#include <bl31/interrupt_mgmt.h>
+#include <context.h>
+#include <lib/psci/psci.h>
+
+/*
+ * This flag is used by the TLKD to determine if the SP is servicing a yielding
+ * SMC request prior to programming the next entry into the SP e.g. if SP
+ * execution is preempted by a non-secure interrupt and handed control to the
+ * normal world. If another request which is distinct from what the SP was
+ * previously doing arrives, then this flag will be help the TLKD to either
+ * reject the new request or service it while ensuring that the previous context
+ * is not corrupted.
+ */
+#define YIELD_SMC_ACTIVE_FLAG_SHIFT	2
+#define YIELD_SMC_ACTIVE_FLAG_MASK	1
+#define get_yield_smc_active_flag(state)				\
+			(((state) >> YIELD_SMC_ACTIVE_FLAG_SHIFT)	\
+			& YIELD_SMC_ACTIVE_FLAG_MASK)
+#define set_yield_smc_active_flag(state)	((state) |=		\
+					 (1 << YIELD_SMC_ACTIVE_FLAG_SHIFT))
+#define clr_yield_smc_active_flag(state)	((state) &=		\
+					 ~(YIELD_SMC_ACTIVE_FLAG_MASK	\
+					 << YIELD_SMC_ACTIVE_FLAG_SHIFT))
+
+/*******************************************************************************
+ * Translate virtual address received from the NS world
+ ******************************************************************************/
+#define TLK_TRANSLATE_NS_VADDR		4
+
+/*******************************************************************************
+ * Secure Payload execution state information i.e. aarch32 or aarch64
+ ******************************************************************************/
+#define SP_AARCH32		MODE_RW_32
+#define SP_AARCH64		MODE_RW_64
+
+/*******************************************************************************
+ * Number of cpus that the present on this platform. TODO: Rely on a topology
+ * tree to determine this in the future to avoid assumptions about mpidr
+ * allocation
+ ******************************************************************************/
+#define TLKD_CORE_COUNT		PLATFORM_CORE_COUNT
+
+/*******************************************************************************
+ * Constants that allow assembler code to preserve callee-saved registers of the
+ * C runtime context while performing a security state switch.
+ ******************************************************************************/
+#define TLKD_C_RT_CTX_X19		0x0
+#define TLKD_C_RT_CTX_X20		0x8
+#define TLKD_C_RT_CTX_X21		0x10
+#define TLKD_C_RT_CTX_X22		0x18
+#define TLKD_C_RT_CTX_X23		0x20
+#define TLKD_C_RT_CTX_X24		0x28
+#define TLKD_C_RT_CTX_X25		0x30
+#define TLKD_C_RT_CTX_X26		0x38
+#define TLKD_C_RT_CTX_X27		0x40
+#define TLKD_C_RT_CTX_X28		0x48
+#define TLKD_C_RT_CTX_X29		0x50
+#define TLKD_C_RT_CTX_X30		0x58
+#define TLKD_C_RT_CTX_SIZE		0x60
+#define TLKD_C_RT_CTX_ENTRIES		(TLKD_C_RT_CTX_SIZE >> DWORD_SHIFT)
+
+#ifndef __ASSEMBLER__
+
+#include <stdint.h>
+
+#include <lib/cassert.h>
+
+/* AArch64 callee saved general purpose register context structure. */
+DEFINE_REG_STRUCT(c_rt_regs, TLKD_C_RT_CTX_ENTRIES);
+
+/*
+ * Compile time assertion to ensure that both the compiler and linker
+ * have the same double word aligned view of the size of the C runtime
+ * register context.
+ */
+CASSERT(TLKD_C_RT_CTX_SIZE == sizeof(c_rt_regs_t),	\
+	assert_tlkd_c_rt_regs_size_mismatch);
+
+/*******************************************************************************
+ * Structure which helps the SPD to maintain the per-cpu state of the SP.
+ * 'state'          - collection of flags to track SP state e.g. on/off
+ * 'mpidr'          - mpidr to associate a context with a cpu
+ * 'c_rt_ctx'       - stack address to restore C runtime context from after
+ *                    returning from a synchronous entry into the SP.
+ * 'cpu_ctx'        - space to maintain SP architectural state
+ * 'saved_tsp_args' - space to store arguments for TSP arithmetic operations
+ *                    which will queried using the TSP_GET_ARGS SMC by TSP.
+ ******************************************************************************/
+typedef struct tlk_context {
+	uint32_t state;
+	uint64_t mpidr;
+	uint64_t c_rt_ctx;
+	cpu_context_t cpu_ctx;
+} tlk_context_t;
+
+/*******************************************************************************
+ * Function & Data prototypes
+ ******************************************************************************/
+uint64_t tlkd_va_translate(uintptr_t va, int type);
+uint64_t tlkd_enter_sp(uint64_t *c_rt_ctx);
+void __dead2 tlkd_exit_sp(uint64_t c_rt_ctx, uint64_t ret);
+uint64_t tlkd_synchronous_sp_entry(tlk_context_t *tlk_ctx);
+void __dead2 tlkd_synchronous_sp_exit(tlk_context_t *tlk_ctx,
+			uint64_t ret);
+void tlkd_init_tlk_ep_state(struct entry_point_info *tlk_entry_point,
+				uint32_t rw,
+				uint64_t pc,
+				tlk_context_t *tlk_ctx);
+
+#endif /*__ASSEMBLER__*/
+
+#endif /* TLKD_PRIVATE_H */

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/trusty/generic-arm64-smcall.c

@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdio.h>
+
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <platform_def.h>
+
+#include "generic-arm64-smcall.h"
+
+#ifndef PLAT_ARM_GICD_BASE
+#ifdef GICD_BASE
+#define PLAT_ARM_GICD_BASE GICD_BASE
+#define PLAT_ARM_GICC_BASE GICC_BASE
+#ifdef GICR_BASE
+#define PLAT_ARM_GICR_BASE GICR_BASE
+#endif
+#else
+#error PLAT_ARM_GICD_BASE or GICD_BASE must be defined
+#endif
+#endif
+
+#ifndef PLAT_ARM_GICR_BASE
+#define PLAT_ARM_GICR_BASE SMC_UNK
+#endif
+
+int trusty_disable_serial_debug;
+
+struct dputc_state {
+	char linebuf[128];
+	unsigned l;
+};
+
+static struct dputc_state dputc_state[2];
+
+static void trusty_dputc(char ch, int secure)
+{
+	unsigned i;
+	struct dputc_state *s = &dputc_state[!secure];
+
+	if (trusty_disable_serial_debug)
+		return;
+
+	s->linebuf[s->l++] = ch;
+	if (s->l == sizeof(s->linebuf) || ch == '\n') {
+		if (secure)
+			printf("secure os: ");
+		else
+			printf("non-secure os: ");
+		for (i = 0; i < s->l; i++) {
+			putchar(s->linebuf[i]);
+		}
+		if (ch != '\n') {
+			printf(" <...>\n");
+		}
+		s->l = 0;
+	}
+}
+
+static uint64_t trusty_get_reg_base(uint32_t reg)
+{
+	switch (reg) {
+	case SMC_GET_GIC_BASE_GICD:
+		return PLAT_ARM_GICD_BASE;
+
+	case SMC_GET_GIC_BASE_GICC:
+		return PLAT_ARM_GICC_BASE;
+
+	case SMC_GET_GIC_BASE_GICR:
+		return PLAT_ARM_GICR_BASE;
+
+	default:
+		NOTICE("%s(0x%x) unknown reg\n", __func__, reg);
+		return SMC_UNK;
+	}
+}
+
+static uintptr_t trusty_generic_platform_smc(uint32_t smc_fid,
+			 u_register_t x1,
+			 u_register_t x2,
+			 u_register_t x3,
+			 u_register_t x4,
+			 void *cookie,
+			 void *handle,
+			 u_register_t flags)
+{
+	switch (smc_fid) {
+	case SMC_FC_DEBUG_PUTC:
+		trusty_dputc(x1, is_caller_secure(flags));
+		SMC_RET1(handle, 0);
+
+	case SMC_FC_GET_REG_BASE:
+	case SMC_FC64_GET_REG_BASE:
+		SMC_RET1(handle, trusty_get_reg_base(x1));
+
+	default:
+		NOTICE("%s(0x%x, 0x%lx) unknown smc\n", __func__, smc_fid, x1);
+		SMC_RET1(handle, SMC_UNK);
+	}
+}
+
+/* Define a SPD runtime service descriptor for fast SMC calls */
+DECLARE_RT_SVC(
+	trusty_fast,
+
+	SMC_ENTITY_PLATFORM_MONITOR,
+	SMC_ENTITY_PLATFORM_MONITOR,
+	SMC_TYPE_FAST,
+	NULL,
+	trusty_generic_platform_smc
+);
+

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/trusty/generic-arm64-smcall.h

@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "smcall.h"
+
+#define	SMC_ENTITY_PLATFORM_MONITOR	61
+
+/*
+ * SMC calls implemented by EL3 monitor
+ */
+
+/*
+ * Write character in r1 to debug console
+ */
+#define SMC_FC_DEBUG_PUTC	SMC_FASTCALL_NR(SMC_ENTITY_PLATFORM_MONITOR, 0x0)
+
+/*
+ * Get register base address
+ * r1: SMC_GET_GIC_BASE_GICD or SMC_GET_GIC_BASE_GICC
+ */
+#define SMC_GET_GIC_BASE_GICD	0
+#define SMC_GET_GIC_BASE_GICC	1
+#define SMC_GET_GIC_BASE_GICR	2
+#define SMC_FC_GET_REG_BASE	SMC_FASTCALL_NR(SMC_ENTITY_PLATFORM_MONITOR, 0x1)
+#define SMC_FC64_GET_REG_BASE	SMC_FASTCALL64_NR(SMC_ENTITY_PLATFORM_MONITOR, 0x1)

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/trusty/sm_err.h

@@ -0,0 +1,22 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SM_ERR_H
+#define SM_ERR_H
+
+/* Errors from the secure monitor */
+#define SM_ERR_UNDEFINED_SMC		0xFFFFFFFF /* Unknown SMC (defined by ARM DEN 0028A(0.9.0) */
+#define SM_ERR_INVALID_PARAMETERS	-2
+#define SM_ERR_INTERRUPTED		-3	/* Got interrupted. Call back with restart SMC */
+#define SM_ERR_UNEXPECTED_RESTART	-4	/* Got an restart SMC when we didn't expect it */
+#define SM_ERR_BUSY			-5	/* Temporarily busy. Call back with original args */
+#define SM_ERR_INTERLEAVED_SMC		-6	/* Got a trusted_service SMC when a restart SMC is required */
+#define SM_ERR_INTERNAL_FAILURE		-7	/* Unknown error */
+#define SM_ERR_NOT_SUPPORTED		-8
+#define SM_ERR_NOT_ALLOWED		-9	/* SMC call not allowed */
+#define SM_ERR_END_OF_INPUT		-10
+
+#endif /* SM_ERR_H */

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/trusty/smcall.h

@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020, NVIDIA Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SMCALL_H
+#define SMCALL_H
+
+#define SMC_NUM_ENTITIES	64U
+#define SMC_NUM_ARGS		4U
+#define SMC_NUM_PARAMS		(SMC_NUM_ARGS - 1U)
+
+#define SMC_IS_FASTCALL(smc_nr)	((smc_nr) & 0x80000000U)
+#define SMC_IS_SMC64(smc_nr)	((smc_nr) & 0x40000000U)
+#define SMC_ENTITY(smc_nr)	(((smc_nr) & 0x3F000000U) >> 24U)
+#define SMC_FUNCTION(smc_nr)	((smc_nr) & 0x0000FFFFU)
+
+#define SMC_NR(entity, fn, fastcall, smc64)			\
+		(((((uint32_t)(fastcall)) & 0x1U) << 31U) |	\
+		(((smc64) & 0x1U) << 30U) |			\
+		(((entity) & 0x3FU) << 24U) |			\
+		((fn) & 0xFFFFU))
+
+#define SMC_FASTCALL_NR(entity, fn)	SMC_NR((entity), (fn), 1U, 0U)
+#define SMC_FASTCALL64_NR(entity, fn)	SMC_NR((entity), (fn), 1U, 1U)
+#define SMC_YIELDCALL_NR(entity, fn)	SMC_NR((entity), (fn), 0U, 0U)
+#define SMC_YIELDCALL64_NR(entity, fn)	SMC_NR((entity), (fn), 0U, 1U)
+
+#define	SMC_ENTITY_ARCH			0U	/* ARM Architecture calls */
+#define	SMC_ENTITY_CPU			1U	/* CPU Service calls */
+#define	SMC_ENTITY_SIP			2U	/* SIP Service calls */
+#define	SMC_ENTITY_OEM			3U	/* OEM Service calls */
+#define	SMC_ENTITY_STD			4U	/* Standard Service calls */
+#define	SMC_ENTITY_RESERVED		5U	/* Reserved for future use */
+#define	SMC_ENTITY_TRUSTED_APP		48U	/* Trusted Application calls */
+#define	SMC_ENTITY_TRUSTED_OS		50U	/* Trusted OS calls */
+#define SMC_ENTITY_LOGGING              51U	/* Used for secure -> nonsecure logging */
+#define	SMC_ENTITY_SECURE_MONITOR	60U	/* Trusted OS calls internal to secure monitor */
+
+/* FC = Fast call, YC = Yielding call */
+#define SMC_YC_RESTART_LAST	SMC_YIELDCALL_NR  (SMC_ENTITY_SECURE_MONITOR, 0U)
+#define SMC_YC_NOP		SMC_YIELDCALL_NR  (SMC_ENTITY_SECURE_MONITOR, 1U)
+
+/*
+ * Return from secure os to non-secure os with return value in r1
+ */
+#define SMC_YC_NS_RETURN	SMC_YIELDCALL_NR  (SMC_ENTITY_SECURE_MONITOR, 0U)
+
+#define SMC_FC_RESERVED		SMC_FASTCALL_NR (SMC_ENTITY_SECURE_MONITOR, 0U)
+#define SMC_FC_FIQ_EXIT		SMC_FASTCALL_NR (SMC_ENTITY_SECURE_MONITOR, 1U)
+#define SMC_FC_REQUEST_FIQ	SMC_FASTCALL_NR (SMC_ENTITY_SECURE_MONITOR, 2U)
+#define SMC_FC_GET_NEXT_IRQ	SMC_FASTCALL_NR (SMC_ENTITY_SECURE_MONITOR, 3U)
+#define SMC_FC_FIQ_ENTER	SMC_FASTCALL_NR (SMC_ENTITY_SECURE_MONITOR, 4U)
+
+#define SMC_FC64_SET_FIQ_HANDLER SMC_FASTCALL64_NR(SMC_ENTITY_SECURE_MONITOR, 5U)
+#define SMC_FC64_GET_FIQ_REGS	SMC_FASTCALL64_NR (SMC_ENTITY_SECURE_MONITOR, 6U)
+
+#define SMC_FC_CPU_SUSPEND	SMC_FASTCALL_NR (SMC_ENTITY_SECURE_MONITOR, 7U)
+#define SMC_FC_CPU_RESUME	SMC_FASTCALL_NR (SMC_ENTITY_SECURE_MONITOR, 8U)
+
+#define SMC_FC_AARCH_SWITCH	SMC_FASTCALL_NR (SMC_ENTITY_SECURE_MONITOR, 9U)
+#define SMC_FC_GET_VERSION_STR	SMC_FASTCALL_NR (SMC_ENTITY_SECURE_MONITOR, 10U)
+
+/* Trusted OS entity calls */
+#define SMC_YC_VIRTIO_GET_DESCR	  SMC_YIELDCALL_NR(SMC_ENTITY_TRUSTED_OS, 20U)
+#define SMC_YC_VIRTIO_START	  SMC_YIELDCALL_NR(SMC_ENTITY_TRUSTED_OS, 21U)
+#define SMC_YC_VIRTIO_STOP	  SMC_YIELDCALL_NR(SMC_ENTITY_TRUSTED_OS, 22U)
+
+#define SMC_YC_VDEV_RESET	  SMC_YIELDCALL_NR(SMC_ENTITY_TRUSTED_OS, 23U)
+#define SMC_YC_VDEV_KICK_VQ	  SMC_YIELDCALL_NR(SMC_ENTITY_TRUSTED_OS, 24U)
+#define SMC_YC_SET_ROT_PARAMS	  SMC_YIELDCALL_NR(SMC_ENTITY_TRUSTED_OS, 65535U)
+
+/*
+ * Standard Trusted OS Function IDs that fall under Trusted OS call range
+ * according to SMC calling convention
+ */
+#define SMC_FC64_GET_UUID		SMC_FASTCALL64_NR(63U, 0xFF01U) /* Implementation UID */
+#define SMC_FC_GET_UUID			SMC_FASTCALL_NR(63U, 0xFF01U) /* Implementation.UID */
+
+#endif /* SMCALL_H */

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/trusty/trusty.c

@@ -0,0 +1,541 @@
+/*
+ * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020, NVIDIA Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <inttypes.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <bl31/bl31.h>
+#include <bl31/interrupt_mgmt.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/smccc.h>
+#include <plat/common/platform.h>
+#include <tools_share/uuid.h>
+
+#include "sm_err.h"
+#include "smcall.h"
+
+/* Trusty UID: RFC-4122 compliant UUID version 4 */
+DEFINE_SVC_UUID2(trusty_uuid,
+		 0x40ee25f0, 0xa2bc, 0x304c, 0x8c, 0x4c,
+		 0xa1, 0x73, 0xc5, 0x7d, 0x8a, 0xf1);
+
+/* macro to check if Hypervisor is enabled in the HCR_EL2 register */
+#define HYP_ENABLE_FLAG		0x286001U
+
+/* length of Trusty's input parameters (in bytes) */
+#define TRUSTY_PARAMS_LEN_BYTES	(4096U * 2)
+
+struct trusty_stack {
+	uint8_t space[PLATFORM_STACK_SIZE] __aligned(16);
+	uint32_t end;
+};
+
+struct trusty_cpu_ctx {
+	cpu_context_t	cpu_ctx;
+	void		*saved_sp;
+	uint32_t	saved_security_state;
+	int32_t		fiq_handler_active;
+	uint64_t	fiq_handler_pc;
+	uint64_t	fiq_handler_cpsr;
+	uint64_t	fiq_handler_sp;
+	uint64_t	fiq_pc;
+	uint64_t	fiq_cpsr;
+	uint64_t	fiq_sp_el1;
+	gp_regs_t	fiq_gpregs;
+	struct trusty_stack	secure_stack;
+};
+
+struct smc_args {
+	uint64_t	r0;
+	uint64_t	r1;
+	uint64_t	r2;
+	uint64_t	r3;
+	uint64_t	r4;
+	uint64_t	r5;
+	uint64_t	r6;
+	uint64_t	r7;
+};
+
+static struct trusty_cpu_ctx trusty_cpu_ctx[PLATFORM_CORE_COUNT];
+
+struct smc_args trusty_init_context_stack(void **sp, void *new_stack);
+struct smc_args trusty_context_switch_helper(void **sp, void *smc_params);
+
+static uint32_t current_vmid;
+
+static struct trusty_cpu_ctx *get_trusty_ctx(void)
+{
+	return &trusty_cpu_ctx[plat_my_core_pos()];
+}
+
+static bool is_hypervisor_mode(void)
+{
+	uint64_t hcr = read_hcr();
+
+	return ((hcr & HYP_ENABLE_FLAG) != 0U) ? true : false;
+}
+
+static struct smc_args trusty_context_switch(uint32_t security_state, uint64_t r0,
+					 uint64_t r1, uint64_t r2, uint64_t r3)
+{
+	struct smc_args args, ret_args;
+	struct trusty_cpu_ctx *ctx = get_trusty_ctx();
+	struct trusty_cpu_ctx *ctx_smc;
+
+	assert(ctx->saved_security_state != security_state);
+
+	args.r7 = 0;
+	if (is_hypervisor_mode()) {
+		/* According to the ARM DEN0028A spec, VMID is stored in x7 */
+		ctx_smc = cm_get_context(NON_SECURE);
+		assert(ctx_smc != NULL);
+		args.r7 = SMC_GET_GP(ctx_smc, CTX_GPREG_X7);
+	}
+	/* r4, r5, r6 reserved for future use. */
+	args.r6 = 0;
+	args.r5 = 0;
+	args.r4 = 0;
+	args.r3 = r3;
+	args.r2 = r2;
+	args.r1 = r1;
+	args.r0 = r0;
+
+	/*
+	 * To avoid the additional overhead in PSCI flow, skip FP context
+	 * saving/restoring in case of CPU suspend and resume, assuming that
+	 * when it's needed the PSCI caller has preserved FP context before
+	 * going here.
+	 */
+	if (r0 != SMC_FC_CPU_SUSPEND && r0 != SMC_FC_CPU_RESUME)
+		fpregs_context_save(get_fpregs_ctx(cm_get_context(security_state)));
+	cm_el1_sysregs_context_save(security_state);
+
+	ctx->saved_security_state = security_state;
+	ret_args = trusty_context_switch_helper(&ctx->saved_sp, &args);
+
+	assert(ctx->saved_security_state == ((security_state == 0U) ? 1U : 0U));
+
+	cm_el1_sysregs_context_restore(security_state);
+	if (r0 != SMC_FC_CPU_SUSPEND && r0 != SMC_FC_CPU_RESUME)
+		fpregs_context_restore(get_fpregs_ctx(cm_get_context(security_state)));
+
+	cm_set_next_eret_context(security_state);
+
+	return ret_args;
+}
+
+static uint64_t trusty_fiq_handler(uint32_t id,
+				   uint32_t flags,
+				   void *handle,
+				   void *cookie)
+{
+	struct smc_args ret;
+	struct trusty_cpu_ctx *ctx = get_trusty_ctx();
+
+	assert(!is_caller_secure(flags));
+
+	ret = trusty_context_switch(NON_SECURE, SMC_FC_FIQ_ENTER, 0, 0, 0);
+	if (ret.r0 != 0U) {
+		SMC_RET0(handle);
+	}
+
+	if (ctx->fiq_handler_active != 0) {
+		INFO("%s: fiq handler already active\n", __func__);
+		SMC_RET0(handle);
+	}
+
+	ctx->fiq_handler_active = 1;
+	(void)memcpy(&ctx->fiq_gpregs, get_gpregs_ctx(handle), sizeof(ctx->fiq_gpregs));
+	ctx->fiq_pc = SMC_GET_EL3(handle, CTX_ELR_EL3);
+	ctx->fiq_cpsr = SMC_GET_EL3(handle, CTX_SPSR_EL3);
+	ctx->fiq_sp_el1 = read_ctx_reg(get_el1_sysregs_ctx(handle), CTX_SP_EL1);
+
+	write_ctx_reg(get_el1_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_handler_sp);
+	cm_set_elr_spsr_el3(NON_SECURE, ctx->fiq_handler_pc, (uint32_t)ctx->fiq_handler_cpsr);
+
+	SMC_RET0(handle);
+}
+
+static uint64_t trusty_set_fiq_handler(void *handle, uint64_t cpu,
+			uint64_t handler, uint64_t stack)
+{
+	struct trusty_cpu_ctx *ctx;
+
+	if (cpu >= (uint64_t)PLATFORM_CORE_COUNT) {
+		ERROR("%s: cpu %" PRId64 " >= %d\n", __func__, cpu, PLATFORM_CORE_COUNT);
+		return (uint64_t)SM_ERR_INVALID_PARAMETERS;
+	}
+
+	ctx = &trusty_cpu_ctx[cpu];
+	ctx->fiq_handler_pc = handler;
+	ctx->fiq_handler_cpsr = SMC_GET_EL3(handle, CTX_SPSR_EL3);
+	ctx->fiq_handler_sp = stack;
+
+	SMC_RET1(handle, 0);
+}
+
+static uint64_t trusty_get_fiq_regs(void *handle)
+{
+	struct trusty_cpu_ctx *ctx = get_trusty_ctx();
+	uint64_t sp_el0 = read_ctx_reg(&ctx->fiq_gpregs, CTX_GPREG_SP_EL0);
+
+	SMC_RET4(handle, ctx->fiq_pc, ctx->fiq_cpsr, sp_el0, ctx->fiq_sp_el1);
+}
+
+static uint64_t trusty_fiq_exit(void *handle, uint64_t x1, uint64_t x2, uint64_t x3)
+{
+	struct smc_args ret;
+	struct trusty_cpu_ctx *ctx = get_trusty_ctx();
+
+	if (ctx->fiq_handler_active == 0) {
+		NOTICE("%s: fiq handler not active\n", __func__);
+		SMC_RET1(handle, (uint64_t)SM_ERR_INVALID_PARAMETERS);
+	}
+
+	ret = trusty_context_switch(NON_SECURE, SMC_FC_FIQ_EXIT, 0, 0, 0);
+	if (ret.r0 != 1U) {
+		INFO("%s(%p) SMC_FC_FIQ_EXIT returned unexpected value, %" PRId64 "\n",
+		       __func__, handle, ret.r0);
+	}
+
+	/*
+	 * Restore register state to state recorded on fiq entry.
+	 *
+	 * x0, sp_el1, pc and cpsr need to be restored because el1 cannot
+	 * restore them.
+	 *
+	 * x1-x4 and x8-x17 need to be restored here because smc_handler64
+	 * corrupts them (el1 code also restored them).
+	 */
+	(void)memcpy(get_gpregs_ctx(handle), &ctx->fiq_gpregs, sizeof(ctx->fiq_gpregs));
+	ctx->fiq_handler_active = 0;
+	write_ctx_reg(get_el1_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_sp_el1);
+	cm_set_elr_spsr_el3(NON_SECURE, ctx->fiq_pc, (uint32_t)ctx->fiq_cpsr);
+
+	SMC_RET0(handle);
+}
+
+static uintptr_t trusty_smc_handler(uint32_t smc_fid,
+			 u_register_t x1,
+			 u_register_t x2,
+			 u_register_t x3,
+			 u_register_t x4,
+			 void *cookie,
+			 void *handle,
+			 u_register_t flags)
+{
+	struct smc_args ret;
+	uint32_t vmid = 0U;
+	entry_point_info_t *ep_info = bl31_plat_get_next_image_ep_info(SECURE);
+
+	/*
+	 * Return success for SET_ROT_PARAMS if Trusty is not present, as
+	 * Verified Boot is not even supported and returning success here
+	 * would not compromise the boot process.
+	 */
+	if ((ep_info == NULL) && (smc_fid == SMC_YC_SET_ROT_PARAMS)) {
+		SMC_RET1(handle, 0);
+	} else if (ep_info == NULL) {
+		SMC_RET1(handle, SMC_UNK);
+	} else {
+		; /* do nothing */
+	}
+
+	if (is_caller_secure(flags)) {
+		if (smc_fid == SMC_YC_NS_RETURN) {
+			ret = trusty_context_switch(SECURE, x1, 0, 0, 0);
+			SMC_RET8(handle, ret.r0, ret.r1, ret.r2, ret.r3,
+				 ret.r4, ret.r5, ret.r6, ret.r7);
+		}
+		INFO("%s (0x%x, 0x%lx, 0x%lx, 0x%lx, 0x%lx, %p, %p, 0x%lx) \
+		     cpu %d, unknown smc\n",
+		     __func__, smc_fid, x1, x2, x3, x4, cookie, handle, flags,
+		     plat_my_core_pos());
+		SMC_RET1(handle, SMC_UNK);
+	} else {
+		switch (smc_fid) {
+		case SMC_FC64_GET_UUID:
+		case SMC_FC_GET_UUID:
+			/* provide the UUID for the service to the client */
+			SMC_UUID_RET(handle, trusty_uuid);
+			break;
+		case SMC_FC64_SET_FIQ_HANDLER:
+			return trusty_set_fiq_handler(handle, x1, x2, x3);
+		case SMC_FC64_GET_FIQ_REGS:
+			return trusty_get_fiq_regs(handle);
+		case SMC_FC_FIQ_EXIT:
+			return trusty_fiq_exit(handle, x1, x2, x3);
+		default:
+			/* Not all OENs greater than SMC_ENTITY_SECURE_MONITOR are supported */
+			if (SMC_ENTITY(smc_fid) > SMC_ENTITY_SECURE_MONITOR) {
+				VERBOSE("%s: unsupported SMC FID (0x%x)\n", __func__, smc_fid);
+				SMC_RET1(handle, SMC_UNK);
+			}
+
+			if (is_hypervisor_mode())
+				vmid = SMC_GET_GP(handle, CTX_GPREG_X7);
+
+			if ((current_vmid != 0) && (current_vmid != vmid)) {
+				/* This message will cause SMC mechanism
+				 * abnormal in multi-guest environment.
+				 * Change it to WARN in case you need it.
+				 */
+				VERBOSE("Previous SMC not finished.\n");
+				SMC_RET1(handle, SM_ERR_BUSY);
+			}
+			current_vmid = vmid;
+			ret = trusty_context_switch(NON_SECURE, smc_fid, x1,
+				x2, x3);
+			current_vmid = 0;
+			SMC_RET1(handle, ret.r0);
+		}
+	}
+}
+
+static int32_t trusty_init(void)
+{
+	entry_point_info_t *ep_info;
+	struct smc_args zero_args = {0};
+	struct trusty_cpu_ctx *ctx = get_trusty_ctx();
+	uint32_t cpu = plat_my_core_pos();
+	uint64_t reg_width = GET_RW(read_ctx_reg(get_el3state_ctx(&ctx->cpu_ctx),
+			       CTX_SPSR_EL3));
+
+	/*
+	 * Get information about the Trusty image. Its absence is a critical
+	 * failure.
+	 */
+	ep_info = bl31_plat_get_next_image_ep_info(SECURE);
+	assert(ep_info != NULL);
+
+	fpregs_context_save(get_fpregs_ctx(cm_get_context(NON_SECURE)));
+	cm_el1_sysregs_context_save(NON_SECURE);
+
+	cm_set_context(&ctx->cpu_ctx, SECURE);
+	cm_init_my_context(ep_info);
+
+	/*
+	 * Adjust secondary cpu entry point for 32 bit images to the
+	 * end of exception vectors
+	 */
+	if ((cpu != 0U) && (reg_width == MODE_RW_32)) {
+		INFO("trusty: cpu %d, adjust entry point to 0x%lx\n",
+		     cpu, ep_info->pc + (1U << 5));
+		cm_set_elr_el3(SECURE, ep_info->pc + (1U << 5));
+	}
+
+	cm_el1_sysregs_context_restore(SECURE);
+	fpregs_context_restore(get_fpregs_ctx(cm_get_context(SECURE)));
+	cm_set_next_eret_context(SECURE);
+
+	ctx->saved_security_state = ~0U; /* initial saved state is invalid */
+	(void)trusty_init_context_stack(&ctx->saved_sp, &ctx->secure_stack.end);
+
+	(void)trusty_context_switch_helper(&ctx->saved_sp, &zero_args);
+
+	cm_el1_sysregs_context_restore(NON_SECURE);
+	fpregs_context_restore(get_fpregs_ctx(cm_get_context(NON_SECURE)));
+	cm_set_next_eret_context(NON_SECURE);
+
+	return 1;
+}
+
+static void trusty_cpu_suspend(uint32_t off)
+{
+	struct smc_args ret;
+
+	ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_SUSPEND, off, 0, 0);
+	if (ret.r0 != 0U) {
+		INFO("%s: cpu %d, SMC_FC_CPU_SUSPEND returned unexpected value, %" PRId64 "\n",
+		     __func__, plat_my_core_pos(), ret.r0);
+	}
+}
+
+static void trusty_cpu_resume(uint32_t on)
+{
+	struct smc_args ret;
+
+	ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_RESUME, on, 0, 0);
+	if (ret.r0 != 0U) {
+		INFO("%s: cpu %d, SMC_FC_CPU_RESUME returned unexpected value, %" PRId64 "\n",
+		     __func__, plat_my_core_pos(), ret.r0);
+	}
+}
+
+static int32_t trusty_cpu_off_handler(u_register_t max_off_lvl)
+{
+	trusty_cpu_suspend(max_off_lvl);
+
+	return 0;
+}
+
+static void trusty_cpu_on_finish_handler(u_register_t max_off_lvl)
+{
+	struct trusty_cpu_ctx *ctx = get_trusty_ctx();
+
+	if (ctx->saved_sp == NULL) {
+		(void)trusty_init();
+	} else {
+		trusty_cpu_resume(max_off_lvl);
+	}
+}
+
+static void trusty_cpu_suspend_handler(u_register_t max_off_lvl)
+{
+	trusty_cpu_suspend(max_off_lvl);
+}
+
+static void trusty_cpu_suspend_finish_handler(u_register_t max_off_lvl)
+{
+	trusty_cpu_resume(max_off_lvl);
+}
+
+static const spd_pm_ops_t trusty_pm = {
+	.svc_off = trusty_cpu_off_handler,
+	.svc_suspend = trusty_cpu_suspend_handler,
+	.svc_on_finish = trusty_cpu_on_finish_handler,
+	.svc_suspend_finish = trusty_cpu_suspend_finish_handler,
+};
+
+void plat_trusty_set_boot_args(aapcs64_params_t *args);
+
+#if !defined(TSP_SEC_MEM_SIZE) && defined(BL32_MEM_SIZE)
+#define TSP_SEC_MEM_SIZE BL32_MEM_SIZE
+#endif
+
+#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;
+	int32_t ret;
+	bool aarch32 = false;
+
+	/* Get trusty's entry point info */
+	ep_info = bl31_plat_get_next_image_ep_info(SECURE);
+	if (ep_info == NULL) {
+		VERBOSE("Trusty image missing.\n");
+		return -1;
+	}
+
+	/* memmap first page of trusty's code memory before peeking */
+	ret = mmap_add_dynamic_region(ep_info->pc, /* PA */
+			ep_info->pc, /* VA */
+			PAGE_SIZE, /* size */
+			MT_SECURE | MT_RW_DATA); /* attrs */
+	assert(ret == 0);
+
+	/* peek into trusty's code to see if we have a 32-bit or 64-bit image */
+	instr = *(uint32_t *)ep_info->pc;
+
+	if (instr >> 24 == 0xeaU) {
+		INFO("trusty: Found 32 bit image\n");
+		aarch32 = true;
+	} else if (instr >> 8 == 0xd53810U || instr >> 16 == 0x9400U) {
+		INFO("trusty: Found 64 bit image\n");
+	} else {
+		ERROR("trusty: Found unknown image, 0x%x\n", instr);
+		return -1;
+	}
+
+	/* unmap trusty's memory page */
+	(void)mmap_remove_dynamic_region(ep_info->pc, PAGE_SIZE);
+
+	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);
+	(void)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);
+
+	/* register power management hooks */
+	psci_register_spd_pm_hook(&trusty_pm);
+
+	/* register interrupt handler */
+	flags = 0;
+	set_interrupt_rm_flag(flags, NON_SECURE);
+	ret = register_interrupt_type_handler(INTR_TYPE_S_EL1,
+					      trusty_fiq_handler,
+					      flags);
+	if (ret != 0) {
+		VERBOSE("trusty: failed to register fiq handler, ret = %d\n", ret);
+	}
+
+	if (aarch32) {
+		entry_point_info_t *ns_ep_info;
+		uint32_t spsr;
+
+		ns_ep_info = bl31_plat_get_next_image_ep_info(NON_SECURE);
+		if (ns_ep_info == NULL) {
+			NOTICE("Trusty: non-secure image missing.\n");
+			return -1;
+		}
+		spsr = ns_ep_info->spsr;
+		if (GET_RW(spsr) == MODE_RW_64 && GET_EL(spsr) == MODE_EL2) {
+			spsr &= ~(MODE_EL_MASK << MODE_EL_SHIFT);
+			spsr |= MODE_EL1 << MODE_EL_SHIFT;
+		}
+		if (GET_RW(spsr) == MODE_RW_32 && GET_M32(spsr) == MODE32_hyp) {
+			spsr &= ~(MODE32_MASK << MODE32_SHIFT);
+			spsr |= MODE32_svc << MODE32_SHIFT;
+		}
+		if (spsr != ns_ep_info->spsr) {
+			NOTICE("Trusty: Switch bl33 from EL2 to EL1 (spsr 0x%x -> 0x%x)\n",
+			       ns_ep_info->spsr, spsr);
+			ns_ep_info->spsr = spsr;
+		}
+	}
+
+	return 0;
+}
+
+/* Define a SPD runtime service descriptor for fast SMC calls */
+DECLARE_RT_SVC(
+	trusty_fast,
+
+	OEN_TOS_START,
+	OEN_TOS_END,
+	SMC_TYPE_FAST,
+	trusty_setup,
+	trusty_smc_handler
+);
+
+/* Define a SPD runtime service descriptor for yielding SMC calls */
+DECLARE_RT_SVC(
+	trusty_std,
+
+	OEN_TAP_START,
+	SMC_ENTITY_SECURE_MONITOR,
+	SMC_TYPE_YIELD,
+	NULL,
+	trusty_smc_handler
+);

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/trusty/trusty.mk

@@ -0,0 +1,18 @@
+#
+# Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+SPD_INCLUDES		:=
+
+SPD_SOURCES		:=	services/spd/trusty/trusty.c		\
+				services/spd/trusty/trusty_helpers.S
+
+ifeq (${TRUSTY_SPD_WITH_GENERIC_SERVICES},1)
+SPD_SOURCES		+=	services/spd/trusty/generic-arm64-smcall.c
+endif
+
+NEED_BL32		:=	yes
+
+CTX_INCLUDE_FPREGS	:=	1

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/trusty/trusty_helpers.S

@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+
+.macro push ra, rb, sp=sp
+	stp \ra, \rb, [\sp,#-16]!
+.endm
+
+.macro pop ra, rb, sp=sp
+	ldp \ra, \rb, [\sp], #16
+.endm
+
+	.global trusty_context_switch_helper
+func trusty_context_switch_helper
+	push	x8, xzr
+	push	x19, x20
+	push	x21, x22
+	push	x23, x24
+	push	x25, x26
+	push	x27, x28
+	push	x29, x30
+
+	mov	x9, sp
+	ldr	x10, [x0]
+	mov	sp, x10
+	str	x9, [x0]
+
+	pop	x29, x30
+	pop	x27, x28
+	pop	x25, x26
+	pop	x23, x24
+	pop	x21, x22
+	pop	x19, x20
+	pop	x8, xzr
+
+        ldr     x2, [x1]
+        ldr     x3, [x1, #0x08]
+        ldr     x4, [x1, #0x10]
+        ldr     x5, [x1, #0x18]
+        ldr     x6, [x1, #0x20]
+        ldr     x7, [x1, #0x28]
+        ldr     x10, [x1, #0x30]
+        ldr     x11, [x1, #0x38]
+
+        stp     x2, x3, [x8]
+        stp     x4, x5, [x8, #16]
+        stp     x6, x7, [x8, #32]
+        stp     x10, x11, [x8, #48]
+
+	ret
+endfunc trusty_context_switch_helper
+
+	.global trusty_init_context_stack
+func trusty_init_context_stack
+	push	x8, xzr, x1
+	push	xzr, xzr, x1
+	push	xzr, xzr, x1
+	push	xzr, xzr, x1
+	push	xzr, xzr, x1
+	push	xzr, xzr, x1
+	adr	x9, el3_exit
+	push	xzr, x9, x1
+	str	x1, [x0]
+	ret
+endfunc trusty_init_context_stack

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/tspd/tspd.mk

@@ -0,0 +1,46 @@
+#
+# Copyright (c) 2013-2018, ARM Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+TSPD_DIR		:=	services/spd/tspd
+
+ifeq (${ERROR_DEPRECATED},0)
+SPD_INCLUDES		:=	-Iinclude/bl32/tsp
+endif
+
+SPD_SOURCES		:=	services/spd/tspd/tspd_common.c		\
+				services/spd/tspd/tspd_helpers.S	\
+				services/spd/tspd/tspd_main.c		\
+				services/spd/tspd/tspd_pm.c
+
+# This dispatcher is paired with a Test Secure Payload source and we intend to
+# build the Test Secure Payload along with this dispatcher.
+#
+# In cases where an associated Secure Payload lies outside this build
+# system/source tree, the the dispatcher Makefile can either invoke an external
+# build command or assume it pre-built
+
+BL32_ROOT		:=	bl32/tsp
+
+# Include SP's Makefile. The assumption is that the TSP's build system is
+# compatible with that of Trusted Firmware, and it'll add and populate necessary
+# build targets and variables
+include ${BL32_ROOT}/tsp.mk
+
+# Let the top-level Makefile know that we intend to build the SP from source
+NEED_BL32		:=	yes
+
+# Flag used to enable routing of non-secure interrupts to EL3 when they are
+# generated while the code is executing in S-EL1/0.
+TSP_NS_INTR_ASYNC_PREEMPT	:=	0
+
+ifeq ($(EL3_EXCEPTION_HANDLING),1)
+ifeq ($(TSP_NS_INTR_ASYNC_PREEMPT),0)
+$(error When EL3_EXCEPTION_HANDLING=1, TSP_NS_INTR_ASYNC_PREEMPT must also be 1)
+endif
+endif
+
+$(eval $(call assert_boolean,TSP_NS_INTR_ASYNC_PREEMPT))
+$(eval $(call add_define,TSP_NS_INTR_ASYNC_PREEMPT))

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/tspd/tspd_common.c

@@ -0,0 +1,140 @@
+/*
+ * Copyright (c) 2013-2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <bl32/tsp/tsp.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/utils.h>
+
+#include "tspd_private.h"
+
+/*******************************************************************************
+ * Given a secure payload entrypoint info pointer, entry point PC, register
+ * width, cpu id & pointer to a context data structure, this function will
+ * initialize tsp context and entry point info for the secure payload
+ ******************************************************************************/
+void tspd_init_tsp_ep_state(struct entry_point_info *tsp_entry_point,
+				uint32_t rw,
+				uint64_t pc,
+				tsp_context_t *tsp_ctx)
+{
+	uint32_t ep_attr;
+
+	/* Passing a NULL context is a critical programming error */
+	assert(tsp_ctx);
+	assert(tsp_entry_point);
+	assert(pc);
+
+	/*
+	 * We support AArch64 TSP for now.
+	 * TODO: Add support for AArch32 TSP
+	 */
+	assert(rw == TSP_AARCH64);
+
+	/* Associate this context with the cpu specified */
+	tsp_ctx->mpidr = read_mpidr_el1();
+	tsp_ctx->state = 0;
+	set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_OFF);
+	clr_yield_smc_active_flag(tsp_ctx->state);
+
+	cm_set_context(&tsp_ctx->cpu_ctx, SECURE);
+
+	/* initialise an entrypoint to set up the CPU context */
+	ep_attr = SECURE | EP_ST_ENABLE;
+	if (read_sctlr_el3() & SCTLR_EE_BIT)
+		ep_attr |= EP_EE_BIG;
+	SET_PARAM_HEAD(tsp_entry_point, PARAM_EP, VERSION_1, ep_attr);
+
+	tsp_entry_point->pc = pc;
+	tsp_entry_point->spsr = SPSR_64(MODE_EL1,
+					MODE_SP_ELX,
+					DISABLE_ALL_EXCEPTIONS);
+	zeromem(&tsp_entry_point->args, sizeof(tsp_entry_point->args));
+}
+
+/*******************************************************************************
+ * This function takes an SP context pointer and:
+ * 1. Applies the S-EL1 system register context from tsp_ctx->cpu_ctx.
+ * 2. Saves the current C runtime state (callee saved registers) on the stack
+ *    frame and saves a reference to this state.
+ * 3. Calls el3_exit() so that the EL3 system and general purpose registers
+ *    from the tsp_ctx->cpu_ctx are used to enter the secure payload image.
+ ******************************************************************************/
+uint64_t tspd_synchronous_sp_entry(tsp_context_t *tsp_ctx)
+{
+	uint64_t rc;
+
+	assert(tsp_ctx != NULL);
+	assert(tsp_ctx->c_rt_ctx == 0);
+
+	/* Apply the Secure EL1 system register context and switch to it */
+	assert(cm_get_context(SECURE) == &tsp_ctx->cpu_ctx);
+	cm_el1_sysregs_context_restore(SECURE);
+	cm_set_next_eret_context(SECURE);
+
+	rc = tspd_enter_sp(&tsp_ctx->c_rt_ctx);
+#if ENABLE_ASSERTIONS
+	tsp_ctx->c_rt_ctx = 0;
+#endif
+
+	return rc;
+}
+
+
+/*******************************************************************************
+ * This function takes an SP context pointer and:
+ * 1. Saves the S-EL1 system register context tp tsp_ctx->cpu_ctx.
+ * 2. Restores the current C runtime state (callee saved registers) from the
+ *    stack frame using the reference to this state saved in tspd_enter_sp().
+ * 3. It does not need to save any general purpose or EL3 system register state
+ *    as the generic smc entry routine should have saved those.
+ ******************************************************************************/
+void tspd_synchronous_sp_exit(tsp_context_t *tsp_ctx, uint64_t ret)
+{
+	assert(tsp_ctx != NULL);
+	/* Save the Secure EL1 system register context */
+	assert(cm_get_context(SECURE) == &tsp_ctx->cpu_ctx);
+	cm_el1_sysregs_context_save(SECURE);
+
+	assert(tsp_ctx->c_rt_ctx != 0);
+	tspd_exit_sp(tsp_ctx->c_rt_ctx, ret);
+
+	/* Should never reach here */
+	assert(0);
+}
+
+/*******************************************************************************
+ * This function takes an SP context pointer and abort any preempted SMC
+ * request.
+ * Return 1 if there was a preempted SMC request, 0 otherwise.
+ ******************************************************************************/
+int tspd_abort_preempted_smc(tsp_context_t *tsp_ctx)
+{
+	if (!get_yield_smc_active_flag(tsp_ctx->state))
+		return 0;
+
+	/* Abort any preempted SMC request */
+	clr_yield_smc_active_flag(tsp_ctx->state);
+
+	/*
+	 * Arrange for an entry into the test secure payload. It will
+	 * be returned via TSP_ABORT_DONE case in tspd_smc_handler.
+	 */
+	cm_set_elr_el3(SECURE,
+		       (uint64_t) &tsp_vectors->abort_yield_smc_entry);
+	uint64_t rc = tspd_synchronous_sp_entry(tsp_ctx);
+
+	if (rc != 0)
+		panic();
+
+	return 1;
+}
+

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/tspd/tspd_helpers.S

@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+#include "tspd_private.h"
+
+	.global	tspd_enter_sp
+	/* ---------------------------------------------
+	 * This function is called with SP_EL0 as stack.
+	 * Here we stash our EL3 callee-saved registers
+	 * on to the stack as a part of saving the C
+	 * runtime and enter the secure payload.
+	 * 'x0' contains a pointer to the memory where
+	 * the address of the C runtime context is to be
+	 * saved.
+	 * ---------------------------------------------
+	 */
+func tspd_enter_sp
+	/* Make space for the registers that we're going to save */
+	mov	x3, sp
+	str	x3, [x0, #0]
+	sub	sp, sp, #TSPD_C_RT_CTX_SIZE
+
+	/* Save callee-saved registers on to the stack */
+	stp	x19, x20, [sp, #TSPD_C_RT_CTX_X19]
+	stp	x21, x22, [sp, #TSPD_C_RT_CTX_X21]
+	stp	x23, x24, [sp, #TSPD_C_RT_CTX_X23]
+	stp	x25, x26, [sp, #TSPD_C_RT_CTX_X25]
+	stp	x27, x28, [sp, #TSPD_C_RT_CTX_X27]
+	stp	x29, x30, [sp, #TSPD_C_RT_CTX_X29]
+
+	/* ---------------------------------------------
+	 * Everything is setup now. el3_exit() will
+	 * use the secure context to restore to the
+	 * general purpose and EL3 system registers to
+	 * ERET into the secure payload.
+	 * ---------------------------------------------
+	 */
+	b	el3_exit
+endfunc tspd_enter_sp
+
+	/* ---------------------------------------------
+	 * This function is called 'x0' pointing to a C
+	 * runtime context saved in tspd_enter_sp().  It
+	 * restores the saved registers and jumps to
+	 * that runtime with 'x0' as the new sp. This
+	 * destroys the C runtime context that had been
+	 * built on the stack below the saved context by
+	 * the caller. Later the second parameter 'x1'
+	 * is passed as return value to the caller
+	 * ---------------------------------------------
+	 */
+	.global tspd_exit_sp
+func tspd_exit_sp
+	/* Restore the previous stack */
+	mov	sp, x0
+
+	/* Restore callee-saved registers on to the stack */
+	ldp	x19, x20, [x0, #(TSPD_C_RT_CTX_X19 - TSPD_C_RT_CTX_SIZE)]
+	ldp	x21, x22, [x0, #(TSPD_C_RT_CTX_X21 - TSPD_C_RT_CTX_SIZE)]
+	ldp	x23, x24, [x0, #(TSPD_C_RT_CTX_X23 - TSPD_C_RT_CTX_SIZE)]
+	ldp	x25, x26, [x0, #(TSPD_C_RT_CTX_X25 - TSPD_C_RT_CTX_SIZE)]
+	ldp	x27, x28, [x0, #(TSPD_C_RT_CTX_X27 - TSPD_C_RT_CTX_SIZE)]
+	ldp	x29, x30, [x0, #(TSPD_C_RT_CTX_X29 - TSPD_C_RT_CTX_SIZE)]
+
+	/* ---------------------------------------------
+	 * This should take us back to the instruction
+	 * after the call to the last tspd_enter_sp().
+	 * Place the second parameter to x0 so that the
+	 * caller will see it as a return value from the
+	 * original entry call
+	 * ---------------------------------------------
+	 */
+	mov	x0, x1
+	ret
+endfunc tspd_exit_sp

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/tspd/tspd_main.c

@@ -0,0 +1,819 @@
+/*
+ * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+
+/*******************************************************************************
+ * This is the Secure Payload Dispatcher (SPD). The dispatcher is meant to be a
+ * plug-in component to the Secure Monitor, registered as a runtime service. The
+ * SPD is expected to be a functional extension of the Secure Payload (SP) that
+ * executes in Secure EL1. The Secure Monitor will delegate all SMCs targeting
+ * the Trusted OS/Applications range to the dispatcher. The SPD will either
+ * handle the request locally or delegate it to the Secure Payload. It is also
+ * responsible for initialising and maintaining communication with the SP.
+ ******************************************************************************/
+#include <assert.h>
+#include <errno.h>
+#include <stddef.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <bl31/bl31.h>
+#include <bl31/ehf.h>
+#include <bl32/tsp/tsp.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <plat/common/platform.h>
+#include <tools_share/uuid.h>
+
+#include "tspd_private.h"
+
+/*******************************************************************************
+ * Address of the entrypoint vector table in the Secure Payload. It is
+ * initialised once on the primary core after a cold boot.
+ ******************************************************************************/
+tsp_vectors_t *tsp_vectors;
+
+/*******************************************************************************
+ * Array to keep track of per-cpu Secure Payload state
+ ******************************************************************************/
+tsp_context_t tspd_sp_context[TSPD_CORE_COUNT];
+
+
+/* TSP UID */
+DEFINE_SVC_UUID2(tsp_uuid,
+	0xa056305b, 0x9132, 0x7b42, 0x98, 0x11,
+	0x71, 0x68, 0xca, 0x50, 0xf3, 0xfa);
+
+int32_t tspd_init(void);
+
+/*
+ * This helper function handles Secure EL1 preemption. The preemption could be
+ * due Non Secure interrupts or EL3 interrupts. In both the cases we context
+ * switch to the normal world and in case of EL3 interrupts, it will again be
+ * routed to EL3 which will get handled at the exception vectors.
+ */
+uint64_t tspd_handle_sp_preemption(void *handle)
+{
+	cpu_context_t *ns_cpu_context;
+
+	assert(handle == cm_get_context(SECURE));
+	cm_el1_sysregs_context_save(SECURE);
+	/* Get a reference to the non-secure context */
+	ns_cpu_context = cm_get_context(NON_SECURE);
+	assert(ns_cpu_context);
+
+	/*
+	 * To allow Secure EL1 interrupt handler to re-enter TSP while TSP
+	 * is preempted, the secure system register context which will get
+	 * overwritten must be additionally saved. This is currently done
+	 * by the TSPD S-EL1 interrupt handler.
+	 */
+
+	/*
+	 * Restore non-secure state.
+	 */
+	cm_el1_sysregs_context_restore(NON_SECURE);
+	cm_set_next_eret_context(NON_SECURE);
+
+	/*
+	 * The TSP was preempted during execution of a Yielding SMC Call.
+	 * Return back to the normal world with SMC_PREEMPTED as error
+	 * code in x0.
+	 */
+	SMC_RET1(ns_cpu_context, SMC_PREEMPTED);
+}
+
+/*******************************************************************************
+ * This function is the handler registered for S-EL1 interrupts by the TSPD. It
+ * validates the interrupt and upon success arranges entry into the TSP at
+ * 'tsp_sel1_intr_entry()' for handling the interrupt.
+ * Typically, interrupts for a specific security state get handled in the same
+ * security execption level if the execution is in the same security state. For
+ * example, if a non-secure interrupt gets fired when CPU is executing in NS-EL2
+ * it gets handled in the non-secure world.
+ * However, interrupts belonging to the opposite security state typically demand
+ * a world(context) switch. This is inline with the security principle which
+ * states a secure interrupt has to be handled in the secure world.
+ * Hence, the TSPD in EL3 expects the context(handle) for a secure interrupt to
+ * be non-secure and vice versa.
+ * However, a race condition between non-secure and secure interrupts can lead to
+ * a scenario where the above assumptions do not hold true. This is demonstrated
+ * below through Note 1.
+ ******************************************************************************/
+static uint64_t tspd_sel1_interrupt_handler(uint32_t id,
+					    uint32_t flags,
+					    void *handle,
+					    void *cookie)
+{
+	uint32_t linear_id;
+	tsp_context_t *tsp_ctx;
+
+	/* Get a reference to this cpu's TSP context */
+	linear_id = plat_my_core_pos();
+	tsp_ctx = &tspd_sp_context[linear_id];
+
+#if TSP_NS_INTR_ASYNC_PREEMPT
+
+	/*
+	 * Note 1:
+	 * Under the current interrupt routing model, interrupts from other
+	 * world are routed to EL3 when TSP_NS_INTR_ASYNC_PREEMPT is enabled.
+	 * Consider the following scenario:
+	 * 1/ A non-secure payload(like tftf) requests a secure service from
+	 *    TSP by invoking a yielding SMC call.
+	 * 2/ Later, execution jumps to TSP in S-EL1 with the help of TSP
+	 *    Dispatcher in Secure Monitor(EL3).
+	 * 3/ While CPU is executing TSP, a Non-secure interrupt gets fired.
+	 *    this demands a context switch to the non-secure world through
+	 *    secure monitor.
+	 * 4/ Consequently, TSP in S-EL1 get asynchronously pre-empted and
+	 *    execution switches to secure monitor(EL3).
+	 * 5/ EL3 tries to triage the (Non-secure) interrupt based on the
+	 *    highest pending interrupt.
+	 * 6/ However, while the NS Interrupt was pending, secure timer gets
+	 *    fired which makes a S-EL1 interrupt to be pending.
+	 * 7/ Hence, execution jumps to this companion handler of S-EL1
+	 *    interrupt (i.e., tspd_sel1_interrupt_handler) even though the TSP
+	 *    was pre-empted due to non-secure interrupt.
+	 * 8/ The above sequence of events explain how TSP was pre-empted by
+	 *    S-EL1 interrupt indirectly in an asynchronous way.
+	 * 9/ Hence, we track the TSP pre-emption by S-EL1 interrupt using a
+	 *    boolean variable per each core.
+	 * 10/ This helps us to indicate that SMC call for TSP service was
+	 *    pre-empted when execution resumes in non-secure world.
+	 */
+
+	/* Check the security state when the exception was generated */
+	if (get_interrupt_src_ss(flags) == NON_SECURE) {
+		/* Sanity check the pointer to this cpu's context */
+		assert(handle == cm_get_context(NON_SECURE));
+
+		/* Save the non-secure context before entering the TSP */
+		cm_el1_sysregs_context_save(NON_SECURE);
+		tsp_ctx->preempted_by_sel1_intr = false;
+	} else {
+		/* Sanity check the pointer to this cpu's context */
+		assert(handle == cm_get_context(SECURE));
+
+		/* Save the secure context before entering the TSP for S-EL1
+		 * interrupt handling
+		 */
+		cm_el1_sysregs_context_save(SECURE);
+		tsp_ctx->preempted_by_sel1_intr = true;
+	}
+#else
+	/* Check the security state when the exception was generated */
+	assert(get_interrupt_src_ss(flags) == NON_SECURE);
+
+	/* Sanity check the pointer to this cpu's context */
+	assert(handle == cm_get_context(NON_SECURE));
+
+	/* Save the non-secure context before entering the TSP */
+	cm_el1_sysregs_context_save(NON_SECURE);
+#endif
+
+	assert(&tsp_ctx->cpu_ctx == cm_get_context(SECURE));
+
+	/*
+	 * Determine if the TSP was previously preempted. Its last known
+	 * context has to be preserved in this case.
+	 * The TSP should return control to the TSPD after handling this
+	 * S-EL1 interrupt. Preserve essential EL3 context to allow entry into
+	 * the TSP at the S-EL1 interrupt entry point using the 'cpu_context'
+	 * structure. There is no need to save the secure system register
+	 * context since the TSP is supposed to preserve it during S-EL1
+	 * interrupt handling.
+	 */
+	if (get_yield_smc_active_flag(tsp_ctx->state)) {
+		tsp_ctx->saved_spsr_el3 = (uint32_t)SMC_GET_EL3(&tsp_ctx->cpu_ctx,
+						      CTX_SPSR_EL3);
+		tsp_ctx->saved_elr_el3 = SMC_GET_EL3(&tsp_ctx->cpu_ctx,
+						     CTX_ELR_EL3);
+#if TSP_NS_INTR_ASYNC_PREEMPT
+		memcpy(&tsp_ctx->sp_ctx, &tsp_ctx->cpu_ctx, TSPD_SP_CTX_SIZE);
+#endif
+	}
+
+	cm_el1_sysregs_context_restore(SECURE);
+	cm_set_elr_spsr_el3(SECURE, (uint64_t) &tsp_vectors->sel1_intr_entry,
+		    SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS));
+
+	cm_set_next_eret_context(SECURE);
+
+	/*
+	 * Tell the TSP that it has to handle a S-EL1 interrupt synchronously.
+	 * Also the instruction in normal world where the interrupt was
+	 * generated is passed for debugging purposes. It is safe to retrieve
+	 * this address from ELR_EL3 as the secure context will not take effect
+	 * until el3_exit().
+	 */
+	SMC_RET2(&tsp_ctx->cpu_ctx, TSP_HANDLE_SEL1_INTR_AND_RETURN, read_elr_el3());
+}
+
+#if TSP_NS_INTR_ASYNC_PREEMPT
+/*******************************************************************************
+ * This function is the handler registered for Non secure interrupts by the
+ * TSPD. It validates the interrupt and upon success arranges entry into the
+ * normal world for handling the interrupt.
+ ******************************************************************************/
+static uint64_t tspd_ns_interrupt_handler(uint32_t id,
+					    uint32_t flags,
+					    void *handle,
+					    void *cookie)
+{
+	/* Check the security state when the exception was generated */
+	assert(get_interrupt_src_ss(flags) == SECURE);
+
+	/*
+	 * Disable the routing of NS interrupts from secure world to EL3 while
+	 * interrupted on this core.
+	 */
+	disable_intr_rm_local(INTR_TYPE_NS, SECURE);
+
+	return tspd_handle_sp_preemption(handle);
+}
+#endif
+
+/*******************************************************************************
+ * Secure Payload Dispatcher setup. The SPD finds out the SP entrypoint and type
+ * (aarch32/aarch64) if not already known and initialises the context for entry
+ * into the SP for its initialisation.
+ ******************************************************************************/
+static int32_t tspd_setup(void)
+{
+	entry_point_info_t *tsp_ep_info;
+	uint32_t linear_id;
+
+	linear_id = plat_my_core_pos();
+
+	/*
+	 * Get information about the Secure Payload (BL32) image. Its
+	 * absence is a critical failure.  TODO: Add support to
+	 * conditionally include the SPD service
+	 */
+	tsp_ep_info = bl31_plat_get_next_image_ep_info(SECURE);
+	if (!tsp_ep_info) {
+		WARN("No TSP provided by BL2 boot loader, Booting device"
+			" without TSP initialization. SMC`s destined for TSP"
+			" will return SMC_UNK\n");
+		return 1;
+	}
+
+	/*
+	 * If there's no valid entry point for SP, we return a non-zero value
+	 * signalling failure initializing the service. We bail out without
+	 * registering any handlers
+	 */
+	if (!tsp_ep_info->pc)
+		return 1;
+
+	/*
+	 * We could inspect the SP image and determine its execution
+	 * state i.e whether AArch32 or AArch64. Assuming it's AArch64
+	 * for the time being.
+	 */
+	tspd_init_tsp_ep_state(tsp_ep_info,
+				TSP_AARCH64,
+				tsp_ep_info->pc,
+				&tspd_sp_context[linear_id]);
+
+#if TSP_INIT_ASYNC
+	bl31_set_next_image_type(SECURE);
+#else
+	/*
+	 * All TSPD initialization done. Now register our init function with
+	 * BL31 for deferred invocation
+	 */
+	bl31_register_bl32_init(&tspd_init);
+#endif
+	return 0;
+}
+
+/*******************************************************************************
+ * This function passes control to the Secure Payload image (BL32) for the first
+ * time on the primary cpu after a cold boot. It assumes that a valid secure
+ * context has already been created by tspd_setup() which can be directly used.
+ * It also assumes that a valid non-secure context has been initialised by PSCI
+ * so it does not need to save and restore any non-secure state. This function
+ * performs a synchronous entry into the Secure payload. The SP passes control
+ * back to this routine through a SMC.
+ ******************************************************************************/
+int32_t tspd_init(void)
+{
+	uint32_t linear_id = plat_my_core_pos();
+	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
+	entry_point_info_t *tsp_entry_point;
+	uint64_t rc;
+
+	/*
+	 * Get information about the Secure Payload (BL32) image. Its
+	 * absence is a critical failure.
+	 */
+	tsp_entry_point = bl31_plat_get_next_image_ep_info(SECURE);
+	assert(tsp_entry_point);
+
+	cm_init_my_context(tsp_entry_point);
+
+	/*
+	 * Arrange for an entry into the test secure payload. It will be
+	 * returned via TSP_ENTRY_DONE case
+	 */
+	rc = tspd_synchronous_sp_entry(tsp_ctx);
+	assert(rc != 0);
+
+	return rc;
+}
+
+
+/*******************************************************************************
+ * This function is responsible for handling all SMCs in the Trusted OS/App
+ * range from the non-secure state as defined in the SMC Calling Convention
+ * Document. It is also responsible for communicating with the Secure payload
+ * to delegate work and return results back to the non-secure state. Lastly it
+ * will also return any information that the secure payload needs to do the
+ * work assigned to it.
+ ******************************************************************************/
+static uintptr_t tspd_smc_handler(uint32_t smc_fid,
+			 u_register_t x1,
+			 u_register_t x2,
+			 u_register_t x3,
+			 u_register_t x4,
+			 void *cookie,
+			 void *handle,
+			 u_register_t flags)
+{
+	cpu_context_t *ns_cpu_context;
+	uint32_t linear_id = plat_my_core_pos(), ns;
+	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
+	uint64_t rc;
+#if TSP_INIT_ASYNC
+	entry_point_info_t *next_image_info;
+#endif
+
+	/* Determine which security state this SMC originated from */
+	ns = is_caller_non_secure(flags);
+
+	switch (smc_fid) {
+
+	/*
+	 * This function ID is used by TSP to indicate that it was
+	 * preempted by a normal world IRQ.
+	 *
+	 */
+	case TSP_PREEMPTED:
+		if (ns)
+			SMC_RET1(handle, SMC_UNK);
+
+		return tspd_handle_sp_preemption(handle);
+
+	/*
+	 * This function ID is used only by the TSP to indicate that it has
+	 * finished handling a S-EL1 interrupt or was preempted by a higher
+	 * priority pending EL3 interrupt. Execution should resume
+	 * in the normal world.
+	 */
+	case TSP_HANDLED_S_EL1_INTR:
+		if (ns)
+			SMC_RET1(handle, SMC_UNK);
+
+		assert(handle == cm_get_context(SECURE));
+
+		/*
+		 * Restore the relevant EL3 state which saved to service
+		 * this SMC.
+		 */
+		if (get_yield_smc_active_flag(tsp_ctx->state)) {
+			SMC_SET_EL3(&tsp_ctx->cpu_ctx,
+				    CTX_SPSR_EL3,
+				    tsp_ctx->saved_spsr_el3);
+			SMC_SET_EL3(&tsp_ctx->cpu_ctx,
+				    CTX_ELR_EL3,
+				    tsp_ctx->saved_elr_el3);
+#if TSP_NS_INTR_ASYNC_PREEMPT
+			/*
+			 * Need to restore the previously interrupted
+			 * secure context.
+			 */
+			memcpy(&tsp_ctx->cpu_ctx, &tsp_ctx->sp_ctx,
+				TSPD_SP_CTX_SIZE);
+#endif
+		}
+
+		/* Get a reference to the non-secure context */
+		ns_cpu_context = cm_get_context(NON_SECURE);
+		assert(ns_cpu_context);
+
+		/*
+		 * Restore non-secure state. There is no need to save the
+		 * secure system register context since the TSP was supposed
+		 * to preserve it during S-EL1 interrupt handling.
+		 */
+		cm_el1_sysregs_context_restore(NON_SECURE);
+		cm_set_next_eret_context(NON_SECURE);
+
+		/* Refer to Note 1 in function tspd_sel1_interrupt_handler()*/
+#if TSP_NS_INTR_ASYNC_PREEMPT
+		if (tsp_ctx->preempted_by_sel1_intr) {
+			/* Reset the flag */
+			tsp_ctx->preempted_by_sel1_intr = false;
+
+			SMC_RET1(ns_cpu_context, SMC_PREEMPTED);
+		} else {
+			SMC_RET0((uint64_t) ns_cpu_context);
+		}
+#else
+		SMC_RET0((uint64_t) ns_cpu_context);
+#endif
+
+
+	/*
+	 * This function ID is used only by the SP to indicate it has
+	 * finished initialising itself after a cold boot
+	 */
+	case TSP_ENTRY_DONE:
+		if (ns)
+			SMC_RET1(handle, SMC_UNK);
+
+		/*
+		 * Stash the SP entry points information. This is done
+		 * only once on the primary cpu
+		 */
+		assert(tsp_vectors == NULL);
+		tsp_vectors = (tsp_vectors_t *) x1;
+
+		if (tsp_vectors) {
+			set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_ON);
+
+			/*
+			 * TSP has been successfully initialized. Register power
+			 * management hooks with PSCI
+			 */
+			psci_register_spd_pm_hook(&tspd_pm);
+
+			/*
+			 * Register an interrupt handler for S-EL1 interrupts
+			 * when generated during code executing in the
+			 * non-secure state.
+			 */
+			flags = 0;
+			set_interrupt_rm_flag(flags, NON_SECURE);
+			rc = register_interrupt_type_handler(INTR_TYPE_S_EL1,
+						tspd_sel1_interrupt_handler,
+						flags);
+			if (rc)
+				panic();
+
+#if TSP_NS_INTR_ASYNC_PREEMPT
+			/*
+			 * Register an interrupt handler for NS interrupts when
+			 * generated during code executing in secure state are
+			 * routed to EL3.
+			 */
+			flags = 0;
+			set_interrupt_rm_flag(flags, SECURE);
+
+			rc = register_interrupt_type_handler(INTR_TYPE_NS,
+						tspd_ns_interrupt_handler,
+						flags);
+			if (rc)
+				panic();
+
+			/*
+			 * Disable the NS interrupt locally.
+			 */
+			disable_intr_rm_local(INTR_TYPE_NS, SECURE);
+#endif
+		}
+
+
+#if TSP_INIT_ASYNC
+		/* Save the Secure EL1 system register context */
+		assert(cm_get_context(SECURE) == &tsp_ctx->cpu_ctx);
+		cm_el1_sysregs_context_save(SECURE);
+
+		/* Program EL3 registers to enable entry into the next EL */
+		next_image_info = bl31_plat_get_next_image_ep_info(NON_SECURE);
+		assert(next_image_info);
+		assert(NON_SECURE ==
+				GET_SECURITY_STATE(next_image_info->h.attr));
+
+		cm_init_my_context(next_image_info);
+		cm_prepare_el3_exit(NON_SECURE);
+		SMC_RET0(cm_get_context(NON_SECURE));
+#else
+		/*
+		 * SP reports completion. The SPD must have initiated
+		 * the original request through a synchronous entry
+		 * into the SP. Jump back to the original C runtime
+		 * context.
+		 */
+		tspd_synchronous_sp_exit(tsp_ctx, x1);
+		break;
+#endif
+	/*
+	 * This function ID is used only by the SP to indicate it has finished
+	 * aborting a preempted Yielding SMC Call.
+	 */
+	case TSP_ABORT_DONE:
+
+	/*
+	 * These function IDs are used only by the SP to indicate it has
+	 * finished:
+	 * 1. turning itself on in response to an earlier psci
+	 *    cpu_on request
+	 * 2. resuming itself after an earlier psci cpu_suspend
+	 *    request.
+	 */
+	case TSP_ON_DONE:
+	case TSP_RESUME_DONE:
+
+	/*
+	 * These function IDs are used only by the SP to indicate it has
+	 * finished:
+	 * 1. suspending itself after an earlier psci cpu_suspend
+	 *    request.
+	 * 2. turning itself off in response to an earlier psci
+	 *    cpu_off request.
+	 */
+	case TSP_OFF_DONE:
+	case TSP_SUSPEND_DONE:
+	case TSP_SYSTEM_OFF_DONE:
+	case TSP_SYSTEM_RESET_DONE:
+		if (ns)
+			SMC_RET1(handle, SMC_UNK);
+
+		/*
+		 * SP reports completion. The SPD must have initiated the
+		 * original request through a synchronous entry into the SP.
+		 * Jump back to the original C runtime context, and pass x1 as
+		 * return value to the caller
+		 */
+		tspd_synchronous_sp_exit(tsp_ctx, x1);
+		break;
+
+		/*
+		 * Request from non-secure client to perform an
+		 * arithmetic operation or response from secure
+		 * payload to an earlier request.
+		 */
+	case TSP_FAST_FID(TSP_ADD):
+	case TSP_FAST_FID(TSP_SUB):
+	case TSP_FAST_FID(TSP_MUL):
+	case TSP_FAST_FID(TSP_DIV):
+
+	case TSP_YIELD_FID(TSP_ADD):
+	case TSP_YIELD_FID(TSP_SUB):
+	case TSP_YIELD_FID(TSP_MUL):
+	case TSP_YIELD_FID(TSP_DIV):
+		/*
+		 * Request from non-secure client to perform a check
+		 * of the DIT PSTATE bit.
+		 */
+	case TSP_YIELD_FID(TSP_CHECK_DIT):
+		if (ns) {
+			/*
+			 * This is a fresh request from the non-secure client.
+			 * The parameters are in x1 and x2. Figure out which
+			 * registers need to be preserved, save the non-secure
+			 * state and send the request to the secure payload.
+			 */
+			assert(handle == cm_get_context(NON_SECURE));
+
+			/* Check if we are already preempted */
+			if (get_yield_smc_active_flag(tsp_ctx->state))
+				SMC_RET1(handle, SMC_UNK);
+
+			cm_el1_sysregs_context_save(NON_SECURE);
+
+			/* Save x1 and x2 for use by TSP_GET_ARGS call below */
+			store_tsp_args(tsp_ctx, x1, x2);
+
+			/*
+			 * We are done stashing the non-secure context. Ask the
+			 * secure payload to do the work now.
+			 */
+
+			/*
+			 * Verify if there is a valid context to use, copy the
+			 * operation type and parameters to the secure context
+			 * and jump to the fast smc entry point in the secure
+			 * payload. Entry into S-EL1 will take place upon exit
+			 * from this function.
+			 */
+			assert(&tsp_ctx->cpu_ctx == cm_get_context(SECURE));
+
+			/* Set appropriate entry for SMC.
+			 * We expect the TSP to manage the PSTATE.I and PSTATE.F
+			 * flags as appropriate.
+			 */
+			if (GET_SMC_TYPE(smc_fid) == SMC_TYPE_FAST) {
+				cm_set_elr_el3(SECURE, (uint64_t)
+						&tsp_vectors->fast_smc_entry);
+			} else {
+				set_yield_smc_active_flag(tsp_ctx->state);
+				cm_set_elr_el3(SECURE, (uint64_t)
+						&tsp_vectors->yield_smc_entry);
+#if TSP_NS_INTR_ASYNC_PREEMPT
+				/*
+				 * Enable the routing of NS interrupts to EL3
+				 * during processing of a Yielding SMC Call on
+				 * this core.
+				 */
+				enable_intr_rm_local(INTR_TYPE_NS, SECURE);
+#endif
+
+#if EL3_EXCEPTION_HANDLING
+				/*
+				 * With EL3 exception handling, while an SMC is
+				 * being processed, Non-secure interrupts can't
+				 * preempt Secure execution. However, for
+				 * yielding SMCs, we want preemption to happen;
+				 * so explicitly allow NS preemption in this
+				 * case, and supply the preemption return code
+				 * for TSP.
+				 */
+				ehf_allow_ns_preemption(TSP_PREEMPTED);
+#endif
+			}
+
+			cm_el1_sysregs_context_restore(SECURE);
+			cm_set_next_eret_context(SECURE);
+			SMC_RET3(&tsp_ctx->cpu_ctx, smc_fid, x1, x2);
+		} else {
+			/*
+			 * This is the result from the secure client of an
+			 * earlier request. The results are in x1-x3. Copy it
+			 * into the non-secure context, save the secure state
+			 * and return to the non-secure state.
+			 */
+			assert(handle == cm_get_context(SECURE));
+			cm_el1_sysregs_context_save(SECURE);
+
+			/* Get a reference to the non-secure context */
+			ns_cpu_context = cm_get_context(NON_SECURE);
+			assert(ns_cpu_context);
+
+			/* Restore non-secure state */
+			cm_el1_sysregs_context_restore(NON_SECURE);
+			cm_set_next_eret_context(NON_SECURE);
+			if (GET_SMC_TYPE(smc_fid) == SMC_TYPE_YIELD) {
+				clr_yield_smc_active_flag(tsp_ctx->state);
+#if TSP_NS_INTR_ASYNC_PREEMPT
+				/*
+				 * Disable the routing of NS interrupts to EL3
+				 * after processing of a Yielding SMC Call on
+				 * this core is finished.
+				 */
+				disable_intr_rm_local(INTR_TYPE_NS, SECURE);
+#endif
+			}
+
+			SMC_RET3(ns_cpu_context, x1, x2, x3);
+		}
+		assert(0); /* Unreachable */
+
+	/*
+	 * Request from the non-secure world to abort a preempted Yielding SMC
+	 * Call.
+	 */
+	case TSP_FID_ABORT:
+		/* ABORT should only be invoked by normal world */
+		if (!ns) {
+			assert(0);
+			break;
+		}
+
+		assert(handle == cm_get_context(NON_SECURE));
+		cm_el1_sysregs_context_save(NON_SECURE);
+
+		/* Abort the preempted SMC request */
+		if (!tspd_abort_preempted_smc(tsp_ctx)) {
+			/*
+			 * If there was no preempted SMC to abort, return
+			 * SMC_UNK.
+			 *
+			 * Restoring the NON_SECURE context is not necessary as
+			 * the synchronous entry did not take place if the
+			 * return code of tspd_abort_preempted_smc is zero.
+			 */
+			cm_set_next_eret_context(NON_SECURE);
+			break;
+		}
+
+		cm_el1_sysregs_context_restore(NON_SECURE);
+		cm_set_next_eret_context(NON_SECURE);
+		SMC_RET1(handle, SMC_OK);
+
+		/*
+		 * Request from non secure world to resume the preempted
+		 * Yielding SMC Call.
+		 */
+	case TSP_FID_RESUME:
+		/* RESUME should be invoked only by normal world */
+		if (!ns) {
+			assert(0);
+			break;
+		}
+
+		/*
+		 * This is a resume request from the non-secure client.
+		 * save the non-secure state and send the request to
+		 * the secure payload.
+		 */
+		assert(handle == cm_get_context(NON_SECURE));
+
+		/* Check if we are already preempted before resume */
+		if (!get_yield_smc_active_flag(tsp_ctx->state))
+			SMC_RET1(handle, SMC_UNK);
+
+		cm_el1_sysregs_context_save(NON_SECURE);
+
+		/*
+		 * We are done stashing the non-secure context. Ask the
+		 * secure payload to do the work now.
+		 */
+#if TSP_NS_INTR_ASYNC_PREEMPT
+		/*
+		 * Enable the routing of NS interrupts to EL3 during resumption
+		 * of a Yielding SMC Call on this core.
+		 */
+		enable_intr_rm_local(INTR_TYPE_NS, SECURE);
+#endif
+
+#if EL3_EXCEPTION_HANDLING
+		/*
+		 * Allow the resumed yielding SMC processing to be preempted by
+		 * Non-secure interrupts. Also, supply the preemption return
+		 * code for TSP.
+		 */
+		ehf_allow_ns_preemption(TSP_PREEMPTED);
+#endif
+
+		/* We just need to return to the preempted point in
+		 * TSP and the execution will resume as normal.
+		 */
+		cm_el1_sysregs_context_restore(SECURE);
+		cm_set_next_eret_context(SECURE);
+		SMC_RET0(&tsp_ctx->cpu_ctx);
+
+		/*
+		 * This is a request from the secure payload for more arguments
+		 * for an ongoing arithmetic operation requested by the
+		 * non-secure world. Simply return the arguments from the non-
+		 * secure client in the original call.
+		 */
+	case TSP_GET_ARGS:
+		if (ns)
+			SMC_RET1(handle, SMC_UNK);
+
+		get_tsp_args(tsp_ctx, x1, x2);
+		SMC_RET2(handle, x1, x2);
+
+	case TOS_CALL_COUNT:
+		/*
+		 * Return the number of service function IDs implemented to
+		 * provide service to non-secure
+		 */
+		SMC_RET1(handle, TSP_NUM_FID);
+
+	case TOS_UID:
+		/* Return TSP UID to the caller */
+		SMC_UUID_RET(handle, tsp_uuid);
+
+	case TOS_CALL_VERSION:
+		/* Return the version of current implementation */
+		SMC_RET2(handle, TSP_VERSION_MAJOR, TSP_VERSION_MINOR);
+
+	default:
+		break;
+	}
+
+	SMC_RET1(handle, SMC_UNK);
+}
+
+/* Define a SPD runtime service descriptor for fast SMC calls */
+DECLARE_RT_SVC(
+	tspd_fast,
+
+	OEN_TOS_START,
+	OEN_TOS_END,
+	SMC_TYPE_FAST,
+	tspd_setup,
+	tspd_smc_handler
+);
+
+/* Define a SPD runtime service descriptor for Yielding SMC Calls */
+DECLARE_RT_SVC(
+	tspd_std,
+
+	OEN_TOS_START,
+	OEN_TOS_END,
+	SMC_TYPE_YIELD,
+	NULL,
+	tspd_smc_handler
+);

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/tspd/tspd_pm.c

@@ -0,0 +1,254 @@
+/*
+ * Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <bl32/tsp/tsp.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <plat/common/platform.h>
+
+#include "tspd_private.h"
+
+/*******************************************************************************
+ * The target cpu is being turned on. Allow the TSPD/TSP to perform any actions
+ * needed. Nothing at the moment.
+ ******************************************************************************/
+static void tspd_cpu_on_handler(u_register_t target_cpu)
+{
+}
+
+/*******************************************************************************
+ * This cpu is being turned off. Allow the TSPD/TSP to perform any actions
+ * needed
+ ******************************************************************************/
+static int32_t tspd_cpu_off_handler(u_register_t unused)
+{
+	int32_t rc = 0;
+	uint32_t linear_id = plat_my_core_pos();
+	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
+
+	assert(tsp_vectors);
+	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_ON);
+
+	/*
+	 * Abort any preempted SMC request before overwriting the SECURE
+	 * context.
+	 */
+	tspd_abort_preempted_smc(tsp_ctx);
+
+	/* Program the entry point and enter the TSP */
+	cm_set_elr_el3(SECURE, (uint64_t) &tsp_vectors->cpu_off_entry);
+	rc = tspd_synchronous_sp_entry(tsp_ctx);
+
+	/*
+	 * Read the response from the TSP. A non-zero return means that
+	 * something went wrong while communicating with the TSP.
+	 */
+	if (rc != 0)
+		panic();
+
+	/*
+	 * Reset TSP's context for a fresh start when this cpu is turned on
+	 * subsequently.
+	 */
+	set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_OFF);
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This cpu is being suspended. S-EL1 state must have been saved in the
+ * resident cpu (mpidr format) if it is a UP/UP migratable TSP.
+ ******************************************************************************/
+static void tspd_cpu_suspend_handler(u_register_t max_off_pwrlvl)
+{
+	int32_t rc = 0;
+	uint32_t linear_id = plat_my_core_pos();
+	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
+
+	assert(tsp_vectors);
+	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_ON);
+
+	/*
+	 * Abort any preempted SMC request before overwriting the SECURE
+	 * context.
+	 */
+	tspd_abort_preempted_smc(tsp_ctx);
+
+	/* Program the entry point and enter the TSP */
+	cm_set_elr_el3(SECURE, (uint64_t) &tsp_vectors->cpu_suspend_entry);
+	rc = tspd_synchronous_sp_entry(tsp_ctx);
+
+	/*
+	 * Read the response from the TSP. A non-zero return means that
+	 * something went wrong while communicating with the TSP.
+	 */
+	if (rc)
+		panic();
+
+	/* Update its context to reflect the state the TSP is in */
+	set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_SUSPEND);
+}
+
+/*******************************************************************************
+ * This cpu has been turned on. Enter the TSP to initialise S-EL1 and other bits
+ * before passing control back to the Secure Monitor. Entry in S-EL1 is done
+ * after initialising minimal architectural state that guarantees safe
+ * execution.
+ ******************************************************************************/
+static void tspd_cpu_on_finish_handler(u_register_t unused)
+{
+	int32_t rc = 0;
+	uint32_t linear_id = plat_my_core_pos();
+	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
+	entry_point_info_t tsp_on_entrypoint;
+
+	assert(tsp_vectors);
+	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_OFF);
+
+	tspd_init_tsp_ep_state(&tsp_on_entrypoint,
+				TSP_AARCH64,
+				(uint64_t) &tsp_vectors->cpu_on_entry,
+				tsp_ctx);
+
+	/* Initialise this cpu's secure context */
+	cm_init_my_context(&tsp_on_entrypoint);
+
+#if TSP_NS_INTR_ASYNC_PREEMPT
+	/*
+	 * Disable the NS interrupt locally since it will be enabled globally
+	 * within cm_init_my_context.
+	 */
+	disable_intr_rm_local(INTR_TYPE_NS, SECURE);
+#endif
+
+	/* Enter the TSP */
+	rc = tspd_synchronous_sp_entry(tsp_ctx);
+
+	/*
+	 * Read the response from the TSP. A non-zero return means that
+	 * something went wrong while communicating with the SP.
+	 */
+	if (rc != 0)
+		panic();
+
+	/* Update its context to reflect the state the SP is in */
+	set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_ON);
+}
+
+/*******************************************************************************
+ * This cpu has resumed from suspend. The SPD saved the TSP context when it
+ * completed the preceding suspend call. Use that context to program an entry
+ * into the TSP to allow it to do any remaining book keeping
+ ******************************************************************************/
+static void tspd_cpu_suspend_finish_handler(u_register_t max_off_pwrlvl)
+{
+	int32_t rc = 0;
+	uint32_t linear_id = plat_my_core_pos();
+	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
+
+	assert(tsp_vectors);
+	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_SUSPEND);
+
+	/* Program the entry point, max_off_pwrlvl and enter the SP */
+	write_ctx_reg(get_gpregs_ctx(&tsp_ctx->cpu_ctx),
+		      CTX_GPREG_X0,
+		      max_off_pwrlvl);
+	cm_set_elr_el3(SECURE, (uint64_t) &tsp_vectors->cpu_resume_entry);
+	rc = tspd_synchronous_sp_entry(tsp_ctx);
+
+	/*
+	 * Read the response from the TSP. A non-zero return means that
+	 * something went wrong while communicating with the TSP.
+	 */
+	if (rc != 0)
+		panic();
+
+	/* Update its context to reflect the state the SP is in */
+	set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_ON);
+}
+
+/*******************************************************************************
+ * Return the type of TSP the TSPD is dealing with. Report the current resident
+ * cpu (mpidr format) if it is a UP/UP migratable TSP.
+ ******************************************************************************/
+static int32_t tspd_cpu_migrate_info(u_register_t *resident_cpu)
+{
+	return TSP_MIGRATE_INFO;
+}
+
+/*******************************************************************************
+ * System is about to be switched off. Allow the TSPD/TSP to perform
+ * any actions needed.
+ ******************************************************************************/
+static void tspd_system_off(void)
+{
+	uint32_t linear_id = plat_my_core_pos();
+	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
+
+	assert(tsp_vectors);
+	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_ON);
+
+	/*
+	 * Abort any preempted SMC request before overwriting the SECURE
+	 * context.
+	 */
+	tspd_abort_preempted_smc(tsp_ctx);
+
+	/* Program the entry point */
+	cm_set_elr_el3(SECURE, (uint64_t) &tsp_vectors->system_off_entry);
+
+	/* Enter the TSP. We do not care about the return value because we
+	 * must continue the shutdown anyway */
+	tspd_synchronous_sp_entry(tsp_ctx);
+}
+
+/*******************************************************************************
+ * System is about to be reset. Allow the TSPD/TSP to perform
+ * any actions needed.
+ ******************************************************************************/
+static void tspd_system_reset(void)
+{
+	uint32_t linear_id = plat_my_core_pos();
+	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
+
+	assert(tsp_vectors);
+	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_ON);
+
+	/*
+	 * Abort any preempted SMC request before overwriting the SECURE
+	 * context.
+	 */
+	tspd_abort_preempted_smc(tsp_ctx);
+
+	/* Program the entry point */
+	cm_set_elr_el3(SECURE, (uint64_t) &tsp_vectors->system_reset_entry);
+
+	/*
+	 * Enter the TSP. We do not care about the return value because we
+	 * must continue the reset anyway
+	 */
+	tspd_synchronous_sp_entry(tsp_ctx);
+}
+
+/*******************************************************************************
+ * Structure populated by the TSP Dispatcher to be given a chance to perform any
+ * TSP bookkeeping before PSCI executes a power mgmt.  operation.
+ ******************************************************************************/
+const spd_pm_ops_t tspd_pm = {
+	.svc_on = tspd_cpu_on_handler,
+	.svc_off = tspd_cpu_off_handler,
+	.svc_suspend = tspd_cpu_suspend_handler,
+	.svc_on_finish = tspd_cpu_on_finish_handler,
+	.svc_suspend_finish = tspd_cpu_suspend_finish_handler,
+	.svc_migrate = NULL,
+	.svc_migrate_info = tspd_cpu_migrate_info,
+	.svc_system_off = tspd_system_off,
+	.svc_system_reset = tspd_system_reset
+};

source/trusted-firmware/trusted-firmware-a-2.8/services/spd/tspd/tspd_private.h

@@ -0,0 +1,233 @@
+/*
+ * Copyright (c) 2013-2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef TSPD_PRIVATE_H
+#define TSPD_PRIVATE_H
+
+#include <platform_def.h>
+
+#include <arch.h>
+#include <bl31/interrupt_mgmt.h>
+#include <context.h>
+#include <lib/psci/psci.h>
+
+/*******************************************************************************
+ * Secure Payload PM state information e.g. SP is suspended, uninitialised etc
+ * and macros to access the state information in the per-cpu 'state' flags
+ ******************************************************************************/
+#define TSP_PSTATE_OFF		0
+#define TSP_PSTATE_ON		1
+#define TSP_PSTATE_SUSPEND	2
+#define TSP_PSTATE_SHIFT	0
+#define TSP_PSTATE_MASK	0x3
+#define get_tsp_pstate(state)	((state >> TSP_PSTATE_SHIFT) & TSP_PSTATE_MASK)
+#define clr_tsp_pstate(state)	(state &= ~(TSP_PSTATE_MASK \
+					    << TSP_PSTATE_SHIFT))
+#define set_tsp_pstate(st, pst)	do {					       \
+					clr_tsp_pstate(st);		       \
+					st |= (pst & TSP_PSTATE_MASK) <<       \
+						TSP_PSTATE_SHIFT;	       \
+				} while (0);
+
+
+/*
+ * This flag is used by the TSPD to determine if the TSP is servicing a yielding
+ * SMC request prior to programming the next entry into the TSP e.g. if TSP
+ * execution is preempted by a non-secure interrupt and handed control to the
+ * normal world. If another request which is distinct from what the TSP was
+ * previously doing arrives, then this flag will be help the TSPD to either
+ * reject the new request or service it while ensuring that the previous context
+ * is not corrupted.
+ */
+#define YIELD_SMC_ACTIVE_FLAG_SHIFT	2
+#define YIELD_SMC_ACTIVE_FLAG_MASK	1
+#define get_yield_smc_active_flag(state)				\
+				((state >> YIELD_SMC_ACTIVE_FLAG_SHIFT) \
+				& YIELD_SMC_ACTIVE_FLAG_MASK)
+#define set_yield_smc_active_flag(state)	(state |=		\
+					1 << YIELD_SMC_ACTIVE_FLAG_SHIFT)
+#define clr_yield_smc_active_flag(state)	(state &=		\
+					~(YIELD_SMC_ACTIVE_FLAG_MASK	\
+					<< YIELD_SMC_ACTIVE_FLAG_SHIFT))
+
+/*******************************************************************************
+ * Secure Payload execution state information i.e. aarch32 or aarch64
+ ******************************************************************************/
+#define TSP_AARCH32		MODE_RW_32
+#define TSP_AARCH64		MODE_RW_64
+
+/*******************************************************************************
+ * The SPD should know the type of Secure Payload.
+ ******************************************************************************/
+#define TSP_TYPE_UP		PSCI_TOS_NOT_UP_MIG_CAP
+#define TSP_TYPE_UPM		PSCI_TOS_UP_MIG_CAP
+#define TSP_TYPE_MP		PSCI_TOS_NOT_PRESENT_MP
+
+/*******************************************************************************
+ * Secure Payload migrate type information as known to the SPD. We assume that
+ * the SPD is dealing with an MP Secure Payload.
+ ******************************************************************************/
+#define TSP_MIGRATE_INFO		TSP_TYPE_MP
+
+/*******************************************************************************
+ * Number of cpus that the present on this platform. TODO: Rely on a topology
+ * tree to determine this in the future to avoid assumptions about mpidr
+ * allocation
+ ******************************************************************************/
+#define TSPD_CORE_COUNT		PLATFORM_CORE_COUNT
+
+/*******************************************************************************
+ * Constants that allow assembler code to preserve callee-saved registers of the
+ * C runtime context while performing a security state switch.
+ ******************************************************************************/
+#define TSPD_C_RT_CTX_X19		0x0
+#define TSPD_C_RT_CTX_X20		0x8
+#define TSPD_C_RT_CTX_X21		0x10
+#define TSPD_C_RT_CTX_X22		0x18
+#define TSPD_C_RT_CTX_X23		0x20
+#define TSPD_C_RT_CTX_X24		0x28
+#define TSPD_C_RT_CTX_X25		0x30
+#define TSPD_C_RT_CTX_X26		0x38
+#define TSPD_C_RT_CTX_X27		0x40
+#define TSPD_C_RT_CTX_X28		0x48
+#define TSPD_C_RT_CTX_X29		0x50
+#define TSPD_C_RT_CTX_X30		0x58
+#define TSPD_C_RT_CTX_SIZE		0x60
+#define TSPD_C_RT_CTX_ENTRIES		(TSPD_C_RT_CTX_SIZE >> DWORD_SHIFT)
+
+/*******************************************************************************
+ * Constants that allow assembler code to preserve caller-saved registers of the
+ * SP context while performing a TSP preemption.
+ * Note: These offsets have to match with the offsets for the corresponding
+ * registers in cpu_context as we are using memcpy to copy the values from
+ * cpu_context to sp_ctx.
+ ******************************************************************************/
+#define TSPD_SP_CTX_X0		0x0
+#define TSPD_SP_CTX_X1		0x8
+#define TSPD_SP_CTX_X2		0x10
+#define TSPD_SP_CTX_X3		0x18
+#define TSPD_SP_CTX_X4		0x20
+#define TSPD_SP_CTX_X5		0x28
+#define TSPD_SP_CTX_X6		0x30
+#define TSPD_SP_CTX_X7		0x38
+#define TSPD_SP_CTX_X8		0x40
+#define TSPD_SP_CTX_X9		0x48
+#define TSPD_SP_CTX_X10		0x50
+#define TSPD_SP_CTX_X11		0x58
+#define TSPD_SP_CTX_X12		0x60
+#define TSPD_SP_CTX_X13		0x68
+#define TSPD_SP_CTX_X14		0x70
+#define TSPD_SP_CTX_X15		0x78
+#define TSPD_SP_CTX_X16		0x80
+#define TSPD_SP_CTX_X17		0x88
+#define TSPD_SP_CTX_SIZE	0x90
+#define TSPD_SP_CTX_ENTRIES		(TSPD_SP_CTX_SIZE >> DWORD_SHIFT)
+
+#ifndef __ASSEMBLER__
+
+#include <stdint.h>
+
+#include <lib/cassert.h>
+
+/*
+ * The number of arguments to save during a SMC call for TSP.
+ * Currently only x1 and x2 are used by TSP.
+ */
+#define TSP_NUM_ARGS	0x2
+
+/* AArch64 callee saved general purpose register context structure. */
+DEFINE_REG_STRUCT(c_rt_regs, TSPD_C_RT_CTX_ENTRIES);
+
+/*
+ * Compile time assertion to ensure that both the compiler and linker
+ * have the same double word aligned view of the size of the C runtime
+ * register context.
+ */
+CASSERT(TSPD_C_RT_CTX_SIZE == sizeof(c_rt_regs_t),	\
+	assert_spd_c_rt_regs_size_mismatch);
+
+/* SEL1 Secure payload (SP) caller saved register context structure. */
+DEFINE_REG_STRUCT(sp_ctx_regs, TSPD_SP_CTX_ENTRIES);
+
+/*
+ * Compile time assertion to ensure that both the compiler and linker
+ * have the same double word aligned view of the size of the C runtime
+ * register context.
+ */
+CASSERT(TSPD_SP_CTX_SIZE == sizeof(sp_ctx_regs_t),	\
+	assert_spd_sp_regs_size_mismatch);
+
+/*******************************************************************************
+ * Structure which helps the SPD to maintain the per-cpu state of the SP.
+ * 'saved_spsr_el3' - temporary copy to allow S-EL1 interrupt handling when
+ *                    the TSP has been preempted.
+ * 'saved_elr_el3'  - temporary copy to allow S-EL1 interrupt handling when
+ *                    the TSP has been preempted.
+ * 'state'          - collection of flags to track SP state e.g. on/off
+ * 'mpidr'          - mpidr to associate a context with a cpu
+ * 'c_rt_ctx'       - stack address to restore C runtime context from after
+ *                    returning from a synchronous entry into the SP.
+ * 'cpu_ctx'        - space to maintain SP architectural state
+ * 'saved_tsp_args' - space to store arguments for TSP arithmetic operations
+ *                    which will queried using the TSP_GET_ARGS SMC by TSP.
+ * 'sp_ctx'         - space to save the SEL1 Secure Payload(SP) caller saved
+ *                    register context after it has been preempted by an EL3
+ *                    routed NS interrupt and when a Secure Interrupt is taken
+ *                    to SP.
+ ******************************************************************************/
+typedef struct tsp_context {
+	uint64_t saved_elr_el3;
+	uint32_t saved_spsr_el3;
+	uint32_t state;
+	uint64_t mpidr;
+	uint64_t c_rt_ctx;
+	cpu_context_t cpu_ctx;
+	uint64_t saved_tsp_args[TSP_NUM_ARGS];
+#if TSP_NS_INTR_ASYNC_PREEMPT
+	sp_ctx_regs_t sp_ctx;
+	bool preempted_by_sel1_intr;
+#endif
+} tsp_context_t;
+
+/* Helper macros to store and retrieve tsp args from tsp_context */
+#define store_tsp_args(_tsp_ctx, _x1, _x2)		do {\
+				_tsp_ctx->saved_tsp_args[0] = _x1;\
+				_tsp_ctx->saved_tsp_args[1] = _x2;\
+			} while (0)
+
+#define get_tsp_args(_tsp_ctx, _x1, _x2)	do {\
+				_x1 = _tsp_ctx->saved_tsp_args[0];\
+				_x2 = _tsp_ctx->saved_tsp_args[1];\
+			} while (0)
+
+/* TSPD power management handlers */
+extern const spd_pm_ops_t tspd_pm;
+
+/*******************************************************************************
+ * Forward declarations
+ ******************************************************************************/
+typedef struct tsp_vectors tsp_vectors_t;
+
+/*******************************************************************************
+ * Function & Data prototypes
+ ******************************************************************************/
+uint64_t tspd_enter_sp(uint64_t *c_rt_ctx);
+void __dead2 tspd_exit_sp(uint64_t c_rt_ctx, uint64_t ret);
+uint64_t tspd_synchronous_sp_entry(tsp_context_t *tsp_ctx);
+void __dead2 tspd_synchronous_sp_exit(tsp_context_t *tsp_ctx, uint64_t ret);
+void tspd_init_tsp_ep_state(struct entry_point_info *tsp_entry_point,
+				uint32_t rw,
+				uint64_t pc,
+				tsp_context_t *tsp_ctx);
+int tspd_abort_preempted_smc(tsp_context_t *tsp_ctx);
+
+uint64_t tspd_handle_sp_preemption(void *handle);
+
+extern tsp_context_t tspd_sp_context[TSPD_CORE_COUNT];
+extern tsp_vectors_t *tsp_vectors;
+#endif /*__ASSEMBLER__*/
+
+#endif /* TSPD_PRIVATE_H */