This is an automated email from the ASF dual-hosted git repository.
GUIDINGLI pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/nuttx.git
The following commit(s) were added to refs/heads/master by this push:
new 61ff9b8b308 nuttx/arch/sm: add the secure monitor setup procedure
61ff9b8b308 is described below
commit 61ff9b8b308e8428adc7e294a3bd05a00e85b2d7
Author: guoshichao <[email protected]>
AuthorDate: Wed Jul 26 19:24:08 2023 +0800
nuttx/arch/sm: add the secure monitor setup procedure
1. add the secure monitor runtime
2. implement ap switch to tee and back with smc/fiq
Signed-off-by: guoshichao <[email protected]>
---
arch/arm/src/armv7-a/CMakeLists.txt | 4 +
arch/arm/src/armv7-a/Kconfig | 6 +
arch/arm/src/armv7-a/Make.defs | 5 +
arch/arm/src/armv7-a/arm_cpuhead.S | 9 +
arch/arm/src/armv7-a/arm_head.S | 9 +
arch/arm/src/armv7-a/arm_sm_runtime.c | 178 +++++++++++
arch/arm/src/armv7-a/arm_sm_vector.S | 544 ++++++++++++++++++++++++++++++++++
arch/arm/src/armv7-a/sm.h | 297 +++++++++++++++++++
8 files changed, 1052 insertions(+)
diff --git a/arch/arm/src/armv7-a/CMakeLists.txt
b/arch/arm/src/armv7-a/CMakeLists.txt
index d9d9ffccd7c..95ccdde9e33 100644
--- a/arch/arm/src/armv7-a/CMakeLists.txt
+++ b/arch/arm/src/armv7-a/CMakeLists.txt
@@ -79,6 +79,10 @@ else()
list(APPEND SRCS arm_head.S)
endif()
+if(CONFIG_ARCH_TRUSTZONE_SECURE)
+ list(APPEND SRCS arm_sm_runtime.c arm_sm_vector.S)
+endif()
+
if(CONFIG_ARCH_ADDRENV)
list(
APPEND
diff --git a/arch/arm/src/armv7-a/Kconfig b/arch/arm/src/armv7-a/Kconfig
index 882dd0ac83a..583b178d96a 100644
--- a/arch/arm/src/armv7-a/Kconfig
+++ b/arch/arm/src/armv7-a/Kconfig
@@ -233,3 +233,9 @@ config ARMV7A_ICACHE_DISABLE
config ARMV7A_AFE_ENABLE
bool "Enable Access Flag at __start"
default n
+
+config ARMV7A_MONITOR_STACK_SIZE
+ int "monitor stack size"
+ default DEFAULT_TASK_STACKSIZE
+ ---help---
+ The size of the monitor stack in armv7-a platform.
diff --git a/arch/arm/src/armv7-a/Make.defs b/arch/arm/src/armv7-a/Make.defs
index 8509c9d56a6..33a684fd1e0 100644
--- a/arch/arm/src/armv7-a/Make.defs
+++ b/arch/arm/src/armv7-a/Make.defs
@@ -66,6 +66,11 @@ else
CMN_ASRCS += arm_head.S
endif
+ifeq ($(CONFIG_ARCH_TRUSTZONE_SECURE),y)
+ CMN_CSRCS += arm_sm_runtime.c
+ CMN_ASRCS += arm_sm_vector.S
+endif
+
ifeq ($(CONFIG_ARCH_ADDRENV),y)
CMN_CSRCS += arm_addrenv.c arm_addrenv_utils.c arm_addrenv_perms.c
arm_pgalloc.c
CMN_CSRCS += arm_addrenv_pgmap.c
diff --git a/arch/arm/src/armv7-a/arm_cpuhead.S
b/arch/arm/src/armv7-a/arm_cpuhead.S
index be95869e3db..262c4908830 100644
--- a/arch/arm/src/armv7-a/arm_cpuhead.S
+++ b/arch/arm/src/armv7-a/arm_cpuhead.S
@@ -450,6 +450,15 @@ __cpu3_start:
bne 1b /* Bottom of the loop */
#endif
+#ifdef CONFIG_ARCH_TRUSTZONE_SECURE
+ /* Perform init the secure monitor runtime. As the following arm_boot
may
+ * using smc to perform psci related func, so the secure monitor init
+ * procedure should be placed before arm_boot
+ */
+
+ bl arm_sm_init
+#endif
+
/* Branch to continue C level CPU initialization */
mov fp, #0 /* Clear framepointer */
diff --git a/arch/arm/src/armv7-a/arm_head.S b/arch/arm/src/armv7-a/arm_head.S
index 6abc2a26e89..b380f924376 100644
--- a/arch/arm/src/armv7-a/arm_head.S
+++ b/arch/arm/src/armv7-a/arm_head.S
@@ -687,6 +687,15 @@ __cpu0_start:
bl arm_data_initialize
#endif
+#ifdef CONFIG_ARCH_TRUSTZONE_SECURE
+ /* Perform init the secure monitor runtime. As the following arm_boot
may
+ * using smc to perform psci related func, so the secure monitor init
+ * procedure should be placed before arm_boot
+ */
+
+ bl arm_sm_init
+#endif
+
/* Perform early C-level, platform-specific initialization. Logic
* within arm_boot() must configure SDRAM and call
arm_data_initialize().
*/
diff --git a/arch/arm/src/armv7-a/arm_sm_runtime.c
b/arch/arm/src/armv7-a/arm_sm_runtime.c
new file mode 100644
index 00000000000..da550a5cd3c
--- /dev/null
+++ b/arch/arm/src/armv7-a/arm_sm_runtime.c
@@ -0,0 +1,178 @@
+/****************************************************************************
+ * arch/arm/src/armv7-a/arm_sm_runtime.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership. The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2015-2023, Linaro Limited
+ * Copyright (c) 2023, Arm Limited
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/arch.h>
+#include <nuttx/irq.h>
+#include <string.h>
+
+#include "arm.h"
+#include "sm.h"
+#include "gic.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define SM_STACK_RESERVE_SIZE sizeof(struct arm_sm_ctx)
+#define MONITOR_STACK_OFFS SM_STACK_RESERVE_SIZE
+#define CFG_MONITOR_STACK_EXTRA 0
+#define MONITOR_STACK_SIZE (CONFIG_ARMV7A_MONITOR_STACK_SIZE + \
+ MONITOR_STACK_OFFS + \
+ CFG_MONITOR_STACK_EXTRA)
+#define STACK_CANARY_SIZE (4 * sizeof(long))
+#define MON_STACK_ALIGNMENT 64 /* (sizeof(long) * U(2)) */
+
+#define ROUNDUP(v, size) (((v) + ((__typeof__(v))(size)-1)) & \
+ ~((__typeof__(v))(size)-1))
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/* the g_monitor_stack are allocated using to manage the secure context and
+ * non-secure context.
+ * the "g_monitor_stack" cannot be declared as "const", as the assembly
+ * code need to access "g_monitor_stack", if we declared as "const",
+ * the arm_sm_init procedure will not work
+ */
+
+#ifdef CONFIG_SMP
+static uint32_t g_monitor_stack[CONFIG_SMP_NCPUS][ROUNDUP(
+ MONITOR_STACK_SIZE + STACK_CANARY_SIZE, MON_STACK_ALIGNMENT) \
+ / sizeof(uint32_t)] \
+ __attribute__((section(".monitor_stack"), \
+ aligned(MON_STACK_ALIGNMENT)));
+#else
+static uint32_t g_monitor_stack[ROUNDUP(
+ MONITOR_STACK_SIZE + STACK_CANARY_SIZE, MON_STACK_ALIGNMENT) \
+ / sizeof(uint32_t)] \
+ __attribute__((section(".monitor_stack"), \
+ aligned(MON_STACK_ALIGNMENT)));
+#endif
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: arm_sm_init
+ *
+ * Description:
+ * This method is called at the boot stage, the arm_head.S will call this
+ * method to perform init the secure monitor runtime
+ *
+ * Input Parameters:
+ * None
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void arm_sm_init(void)
+{
+#ifdef CONFIG_SMP
+ arm_sm_init_stack(((int)&g_monitor_stack[up_cpu_index()]
+ + sizeof(g_monitor_stack[up_cpu_index()]))
+ - MONITOR_STACK_OFFS);
+#else
+ arm_sm_init_stack(((int)&g_monitor_stack + sizeof(g_monitor_stack))
+ - MONITOR_STACK_OFFS);
+#endif
+}
+
+/****************************************************************************
+ * Name: arm_sm_boot_nsec
+ *
+ * Description:
+ * As the ap core is loaded by tee core, when the tee core is boot
+ * finished, it need to setup the nsec_ctc mon_lr as the ap core entry
+ * address, then using the smc command to switch to the monitor mode and
+ * switch to the ap core
+ *
+ * Input Parameters:
+ * entry - The ap core entry address
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void arm_sm_boot_nsec(uintptr_t entry)
+{
+ FAR struct arm_sm_nsec_ctx *nsec_ctx;
+
+ nsec_ctx = arm_sm_get_nsec_ctx();
+ nsec_ctx->mon_lr = entry;
+ nsec_ctx->mon_spsr = PSR_MODE_SVC | PSR_I_BIT;
+ if (entry & 1)
+ {
+ nsec_ctx->mon_spsr |= PSR_T_BIT;
+ }
+}
+
+/****************************************************************************
+ * Name: arm_sm_from_nsec
+ *
+ * Description:
+ * Returns one of SM_EXIT_TO_* exit monitor in secure or non-secure world
+ *
+ * Input Parameters:
+ * ctx - The secure monitor context
+ *
+ * Returned Value:
+ * SM_EXIT_TO_NON_SECURE: return to the non-secure world after this
+ * procedure
+ * SM_EXIT_TO_SECURE: continue running inside the secure world
+ *
+ ****************************************************************************/
+
+uint32_t arm_sm_from_nsec(FAR struct arm_sm_ctx *ctx)
+{
+ FAR uint32_t *nsec_r0 = (FAR uint32_t *)(&ctx->nsec.r0);
+ FAR struct arm_smccc_res *args = (FAR struct arm_smccc_res *)nsec_r0;
+
+ arm_sm_save_banked_regs(&ctx->nsec.regs);
+ arm_sm_restore_banked_regs(&ctx->sec.regs);
+
+ memcpy(&ctx->sec.r0, args, sizeof(*args));
+
+ ctx->sec.mon_lr = (uint32_t)arm_vectorsmc;
+
+ /* the following procedure return "SM_EXIT_TO_SECURE" which indicate
+ * that we should still stay at secure world, until the
+ * "arm_vectorsmc" using "smc #0" to return to the
+ * non-secure world
+ */
+
+ return SM_EXIT_TO_SECURE;
+}
diff --git a/arch/arm/src/armv7-a/arm_sm_vector.S
b/arch/arm/src/armv7-a/arm_sm_vector.S
new file mode 100644
index 00000000000..e36fc995f98
--- /dev/null
+++ b/arch/arm/src/armv7-a/arm_sm_vector.S
@@ -0,0 +1,544 @@
+/****************************************************************************
+ * arch/arm/src/armv7-a/arm_vectorsm.S
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership. The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2016-2020, Linaro Limited
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include "arm.h"
+#include <arch/arm_a_r/cp15.h>
+#include "sctlr.h"
+
+ .file "arm_vectorsm.S"
+
+/****************************************************************************
+ * Configuration
+ ****************************************************************************/
+
+#define SM_EXIT_TO_NON_SECURE 0
+#define SM_CTX_SEC 0 /* offsetof(struct arm_sm_ctx, sec) */
+#define SM_CTX_NSEC 112 /* offsetof(struct arm_sm_ctx, nsec) */
+#define SM_CTX_SIZE 244 /* sizeof(struct arm_sm_ctx) */
+#define SM_CTX_SEC_SIZE 112 /* sizeof(struct arm_sm_sec_ctx) */
+#define SM_CTX_SEC_END (SM_CTX_SEC + SM_CTX_SEC_SIZE)
+#define SM_SEC_CTX_R0 72 /* offsetof(struct arm_sm_sec_ctx, r0)
*/
+#define SM_SEC_CTX_MON_LR 104 /* offsetof(struct arm_sm_sec_ctx,
mon_lr) */
+#define SM_NSEC_CTX_R0 92 /* offsetof(struct arm_sm_nsec_ctx,
r0) */
+#define SM_NSEC_CTX_R8 72 /* offsetof(struct arm_sm_nsec_ctx,
r8) */
+
+#define TEESMC_RETURN_ENTRY_DONE 0
+
+/* Issued when returning from "std_smc" or "fast_smc" vector */
+#define TEESMC_FUNCID_RETURN_CALL_DONE 1
+
+/* Issued when returning from "fiq" vector */
+#define TEESMC_RETURN_FIQ_DONE 2
+
+/****************************************************************************
+ * Assembly Macros
+ ****************************************************************************/
+
+.macro save_regs mode
+ cps \mode
+ mrs r2, spsr
+ str r2, [r0], #4
+ str sp, [r0], #4
+ str lr, [r0], #4
+.endm
+
+.macro restore_regs mode
+ cps \mode
+ ldr r2, [r0], #4
+ ldr sp, [r0], #4
+ ldr lr, [r0], #4
+ msr spsr_fsxc, r2
+.endm
+
+/****************************************************************************
+ * Name: mov_imm
+ *
+ * Parameter:
+ * reg - the dest register to contains the immediate value
+ * val - the immediate value
+ *
+ * Description:
+ * Move the immediate value to the register, but this macro can handle the
+ * case that when the immediate value is larger than 2^16
+ *
+ ****************************************************************************/
+
+.macro mov_imm reg, val
+ .if ((\val) & 0xffff0000) == 0
+ movw \reg, #(\val)
+ .else
+ movw \reg, #((\val) & 0xffff)
+ movt \reg, #((\val) >> 16)
+ .endif
+.endm
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+ .text
+ .syntax unified
+ .arm
+
+/****************************************************************************
+ * Name: arm_sm_vect_start
+ *
+ * Description:
+ * Secure monitor vector initialization block
+ ****************************************************************************/
+
+ .local arm_sm_vect_start
+ .balign 32
+
+arm_sm_vect_start:
+ ldr pc, . /* 0x00: Reset */
+ ldr pc, . /* 0x04: Undefined
instruction */
+ ldr pc, .Lsm_smchandler /* 0x08: Secure monitor call */
+ ldr pc, . /* 0x0c: Prefetch abort
*/
+ ldr pc, . /* 0x10: Data abort */
+ ldr pc, . /* 0x14: Reserved */
+ ldr pc, . /* 0x18: IRQ */
+ ldr pc, .Lsm_fiqhandler /* 0x1c: FIQ */
+
+ .local arm_sm_vectorsmc
+ .local arm_sm_vectorfiq
+
+.Lsm_smchandler:
+ .long arm_sm_vectorsmc
+.Lsm_fiqhandler:
+ .long arm_sm_vectorfiq
+
+ .size arm_sm_vect_start, . - arm_sm_vect_start
+ .align 4
+
+/****************************************************************************
+ * Name: arm_sm_save_banked_regs
+ ****************************************************************************/
+
+ .globl arm_sm_save_banked_regs
+ .type arm_sm_save_banked_regs, %function
+
+arm_sm_save_banked_regs:
+ /* User mode registers has to be saved from system mode */
+
+ cps #PSR_MODE_SYS
+ str sp, [r0], #4
+ str lr, [r0], #4
+
+ save_regs #PSR_MODE_IRQ
+ save_regs #PSR_MODE_FIQ
+ save_regs #PSR_MODE_SVC
+ save_regs #PSR_MODE_ABT
+ save_regs #PSR_MODE_UND
+
+ cps #PSR_MODE_MON
+ bx lr
+
+ .size arm_sm_save_banked_regs, . - arm_sm_save_banked_regs
+ .align 4
+
+/****************************************************************************
+ * Name: arm_sm_restore_banked_regs
+ ****************************************************************************/
+
+/* Restores the mode specific registers */
+
+ .globl arm_sm_restore_banked_regs
+ .type arm_sm_restore_banked_regs, #function
+
+arm_sm_restore_banked_regs:
+ /* User mode registers has to be saved from system mode */
+
+ cps #PSR_MODE_SYS
+ ldr sp, [r0], #4
+ ldr lr, [r0], #4
+
+ restore_regs #PSR_MODE_IRQ
+ restore_regs #PSR_MODE_FIQ
+ restore_regs #PSR_MODE_SVC
+ restore_regs #PSR_MODE_ABT
+ restore_regs #PSR_MODE_UND
+
+ cps #PSR_MODE_MON
+ bx lr
+
+ .size arm_sm_restore_banked_regs, . - arm_sm_restore_banked_regs
+ .align 4
+
+/****************************************************************************
+ * Name: arm_sm_vectorsmc
+ ****************************************************************************/
+
+/* the arm_sm_vectorsmc is called at "smc #0" is invoked. As each world can
+ * trigger the smc call, so this method internal will detect which world
+ * we come from:
+ * if come from non-secure world, will save the non-secure
+ * context, and restore the secure world context, and then call the
+ * "arm_sm_from_nsec" to handle the following work, and after
"arm_sm_from_nsec"
+ * method is processing finished, will take the responsibility to return
+ * to the non-secure world by calling "smc #0";
+ * if come from secure world, will save the secure world context, and then
+ * restore the non-secure world context, and then just perform "rfefd" to
+ * exit the smc exception, and the non-secure world will resume, continue
+ * running.
+ * g_monitor_stack is used as stack, the top of the stack is reserved to hold
+ * struct arm_sm_ctx, everything below is for normal stack usage. As several
+ * different CPU modes are using the same stack it's important that switch
+ * of CPU mode isn't done until one mode is done. This means FIQ, IRQ and
+ * Async abort has to be masked while using g_monitor_stack.
+ */
+
+ .local arm_sm_vectorsmc
+ .type arm_sm_vectorsmc, %function
+
+arm_sm_vectorsmc:
+ srsdb sp!, #PSR_MODE_MON
+ push {r0-r7}
+
+ /* Clear the exclusive monitor */
+
+ clrex
+
+ /* Find out if we're doing an secure or non-secure entry */
+
+ mrc CP15_SCR(r1)
+ tst r1, #SCR_NS
+ bne .smc_from_nsec
+
+ /* As we're coming from secure world (NS bit cleared) the stack
+ * pointer points to arm_sm_ctx.sec.r0 at this stage. After the
+ * instruction below the stack pointer points to arm_sm_ctx.
+ */
+
+ sub sp, sp, #(SM_CTX_SEC + SM_SEC_CTX_R0)
+
+ /* Save secure context */
+
+ add r0, sp, #SM_CTX_SEC
+
+ /* the arm_sm_save_banked_regs() function will execute "cps
#PSR_MODE_MON"
+ * and help switch to monitor mode
+ */
+ bl arm_sm_save_banked_regs
+
+ /* On FIQ exit we're restoring the non-secure context unchanged, on
+ * all other exits we're shifting r1-r4 from secure context into
+ * r0-r3 in non-secure context.
+ */
+
+ add r8, sp, #(SM_CTX_SEC + SM_SEC_CTX_R0)
+ ldm r8, {r0-r4}
+ mov_imm r9, TEESMC_RETURN_FIQ_DONE
+ cmp r0, r9
+ addne r8, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0)
+ stmne r8, {r1-r4}
+
+ /* Restore non-secure context */
+
+ add r0, sp, #SM_CTX_NSEC
+ bl arm_sm_restore_banked_regs
+
+.sm_ret_to_nsec:
+ /* Return to non-secure world */
+
+ add r0, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R8)
+ ldm r0, {r8-r12}
+
+ /* Update SCR */
+
+ mrc CP15_SCR(r0)
+ orr r0, r0, #(SCR_NS | SCR_FIQ) /* Set NS and FIQ bit in SCR */
+ mcr CP15_SCR(r0)
+
+ /* isb not needed since we're doing an exception return below
+ * without dependency to the changes in SCR before that.
+ */
+
+ add sp, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0)
+ b .sm_exit
+
+.smc_from_nsec:
+ /* As we're coming from non-secure world (NS bit set) the stack
+ * pointer points to arm_sm_ctx.nsec.r0 at this stage. After the
+ * instruction below the stack pointer points to arm_sm_ctx.
+ */
+
+ sub sp, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0)
+
+ bic r1, r1, #(SCR_NS | SCR_FIQ) /* Clear NS and FIQ bit in SCR */
+ mcr CP15_SCR(r1)
+ isb
+
+ add r0, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R8)
+ stm r0, {r8-r12}
+
+ mov r0, sp
+ bl arm_sm_from_nsec
+ cmp r0, #SM_EXIT_TO_NON_SECURE
+ beq .sm_ret_to_nsec
+
+ /* Continue into secure world */
+
+ add sp, sp, #(SM_CTX_SEC + SM_SEC_CTX_R0)
+
+.sm_exit:
+ pop {r0-r7}
+ rfefd sp!
+
+ .size arm_sm_vectorsmc, . - arm_sm_vectorsmc
+ .align 4
+
+/****************************************************************************
+ * Name: arm_sm_vectorfiq
+ ****************************************************************************/
+
+/* FIQ handling
+ *
+ * Saves CPU context in the same way as arm_sm_vectorsmc() above. The CPU
+ * context will later be restored by arm_sm_vectorsmc() when handling a return
+ * from FIQ.
+ *
+ * When cpu is running in non-secure mode, then there are fiq is triggered
+ * in secure mode, then the non-secure mode will need to transfer this fiq
+ * to the secure mode, in order to implement this, this fiq will trapped into
+ * monitor mode, and the monitor mode will invoke the arm_sm_vectorfiq func to
+ * handle this fiq. The arm_sm_vectorfiq will save the non-secure context, and
+ * then restore the non-secure context, and then exit current monitor mode,
+ * and then will running the secure mode, then the secure mode will handle
+ * this fiq
+ */
+
+ .local arm_sm_vectorfiq
+ .type arm_sm_vectorfiq, %function
+
+arm_sm_vectorfiq:
+ /* FIQ has a +4 offset for lr compared to preferred return address */
+
+ sub lr, lr, #4
+ /* sp points just past struct arm_sm_sec_ctx */
+
+ srsdb sp!, #PSR_MODE_MON
+ push {r0-r7}
+
+ clrex /* Clear the exclusive monitor */
+
+ /*
+ * As we're coming from non-secure world the stack pointer points
+ * to arm_sm_ctx.nsec.r0 at this stage. After the instruction below the
+ * stack pointer points to arm_sm_ctx.
+ */
+
+ sub sp, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0)
+
+ /* Update SCR */
+
+ mrc CP15_SCR(r1)
+ bic r1, r1, #(SCR_NS | SCR_FIQ) /* Clear NS and FIQ bit in SCR */
+ mcr CP15_SCR(r1)
+ isb
+
+ /* Save non-secure context */
+
+ add r0, sp, #SM_CTX_NSEC
+ bl arm_sm_save_banked_regs
+ add r0, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R8)
+ stm r0!, {r8-r12}
+
+ /* Set FIQ entry */
+ ldr r0, =arm_vectorfiq_entry
+ str r0, [sp, #(SM_CTX_SEC + SM_SEC_CTX_MON_LR)]
+
+ /* Restore secure context */
+
+ add r0, sp, #SM_CTX_SEC
+ bl arm_sm_restore_banked_regs
+
+ /* return to the secure world */
+
+ add sp, sp, #(SM_CTX_SEC + SM_SEC_CTX_MON_LR)
+
+ rfefd sp!
+
+ .size arm_sm_vectorfiq, . - arm_sm_vectorfiq
+ .align 4
+
+/****************************************************************************
+ * Name: arm_vectorsmc
+ ****************************************************************************/
+
+/* this function is declared in sm.h:
+ * void arm_vectorsmc(uint32_t a0, uint32_t a1, uint32_t a2,
+ * uint32_t a3, uint32_t a4, uint32_t a5,
+ * uint32_t a6, uint32_t a7);
+ * There are two ways to switch from REE to TEE, SMC exception and FIQ
exception,
+ * if we use FIQ exception to trap into the MONITOR mode, then before enter to
+ * TEE mode, need to setup the ctx->sec.mon_lr as "arm_vectorfiq_entry", then
the
+ * "arm_vectorfiq_entry" is using as the entry to the TEE mode;
+ * if we use SMC exception to trap into the MONITOR mode, then before enter to
+ * TEE mode, need to setup the ctx->sec.mon_lr as "arm_vectorsmc", then
+ * the "arm_vectorsmc" is using as the entry to the TEE mode;
+ * the "arm_vectorsmc" is working same as "arm_vectorfiq_entry"
+ */
+
+ .global arm_vectorsmc
+ .type arm_vectorsmc, %function
+
+arm_vectorsmc:
+ mov r1, r0
+ ldr r0, =TEESMC_FUNCID_RETURN_CALL_DONE
+ smc #0
+ /* SMC should not return */
+
+ b .
+
+ .size arm_vectorsmc, . - arm_vectorsmc
+ .align 4
+
+/****************************************************************************
+ * Name: arm_vectorfiq_entry
+ ****************************************************************************/
+
+/* this function is declared in sm.h:
+ * void arm_vectorfiq_entry(uint32_t a0, uint32_t a1, uint32_t a2,
+ * uint32_t a3, uint32_t a4, uint32_t a5,
+ * uint32_t a6, uint32_t a7);
+ * Secure Monitor received a FIQ and passed control to us.
+ * When the fiq is process finished in secure world, then the
arm_vectorfiq_entry
+ * function will continue running, then could perform some other actions on
+ * arm_vectorfiq_entry. At the end of the arm_vectorfiq_entry, will call smc to
+ * switch to the non-secure world
+ */
+
+ .local arm_vectorfiq_entry
+ .type arm_vectorfiq_entry, %function
+
+arm_vectorfiq_entry:
+ ldr r0, =TEESMC_RETURN_FIQ_DONE
+ smc #0
+ /* SMC should not return */
+
+ b .
+
+ .size arm_vectorfiq_entry, . - arm_vectorfiq_entry
+ .align 4
+
+/****************************************************************************
+ * Name: arm_sm_init_stack
+ ****************************************************************************/
+
+/* this function is declared in sm.h
+ * void arm_sm_init_stack(int stack);
+ *
+ * this function is using to init the monitor runtime
+ * this function will write the address of arm_sm_vect_start
+ * to the mvbar register
+ */
+
+ .globl arm_sm_init_stack
+ .type arm_sm_init_stack, %function
+
+arm_sm_init_stack:
+ /* Set monitor stack */
+
+ mrs r1, cpsr
+ cps #PSR_MODE_MON
+
+ /* Point just beyond arm_sm_ctx.sec */
+
+ sub sp, r0, #0
+ msr cpsr, r1
+
+ /* Set monitor vector (MVBAR) */
+
+1:
+ ldr r0, =arm_sm_vect_start
+2:
+ mcr CP15_MVBAR(r0)
+
+ bx lr
+
+ .size arm_sm_init_stack, . - arm_sm_init_stack
+ .align 4
+
+/****************************************************************************
+ * Name: arm_sm_get_nsec_ctx
+ ****************************************************************************/
+
+/* This function is declared in sm.h:
+ * struct arm_sm_nsec_ctx *arm_sm_get_nsec_ctx(void);
+ *
+ * this function are using to fetch the non-secure context
+ * the non-secure context are stored in g_monitor_stack
+ */
+
+ .globl arm_sm_get_nsec_ctx
+ .type arm_sm_get_nsec_ctx, %function
+
+arm_sm_get_nsec_ctx:
+ mrs r1, cpsr
+ cps #PSR_MODE_MON
+
+ /* As we're in secure mode mon_sp points just beyond arm_sm_ctx.sec,
+ * which allows us to calculate the address of arm_sm_ctx.nsec.
+ */
+
+ add r0, sp, #0
+ msr cpsr, r1
+
+ bx lr
+
+ .size arm_sm_get_nsec_ctx, . - arm_sm_get_nsec_ctx
+ .align 4
+
+/****************************************************************************
+ * Name: arm_sm_switch_nsec
+ ****************************************************************************/
+
+/* this function is declared in sm.h:
+ * void arm_sm_switch_nsec(void);
+ *
+ * This simple method using to switch to non-secure world at system boot, the
+ * switch is simply by calling "smc, #0"
+ */
+
+ .globl arm_sm_switch_nsec
+ .type arm_sm_switch_nsec, %function
+
+arm_sm_switch_nsec:
+ ldr r0, =TEESMC_RETURN_ENTRY_DONE
+ smc #0
+
+ /* SMC should not return */
+
+ b .
+
+ .size arm_sm_switch_nsec, . - arm_sm_switch_nsec
+ .align 4
diff --git a/arch/arm/src/armv7-a/sm.h b/arch/arm/src/armv7-a/sm.h
new file mode 100644
index 00000000000..deccdced904
--- /dev/null
+++ b/arch/arm/src/armv7-a/sm.h
@@ -0,0 +1,297 @@
+/****************************************************************************
+ * arch/arm/src/armv7-a/sm.h
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership. The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2015-2020, Linaro Limited
+ * Copyright (c) 2021-2023, Arm Limited
+ *
+ ****************************************************************************/
+
+#ifndef __ARCH_ARM_SRC_ARMV7_A_SM_H
+#define __ARCH_ARM_SRC_ARMV7_A_SM_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <stdint.h>
+
+#include "arm_cpu_psci.h"
+#include "sctlr.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define SM_EXIT_TO_SECURE 1
+
+/****************************************************************************
+ * Public Type Declarations
+ ****************************************************************************/
+
+struct arm_sm_banked_regs
+{
+ uint32_t usr_sp;
+ uint32_t usr_lr;
+ uint32_t irq_spsr;
+ uint32_t irq_sp;
+ uint32_t irq_lr;
+ uint32_t fiq_spsr;
+ uint32_t fiq_sp;
+ uint32_t fiq_lr;
+
+ /* Note that fiq_r{8-12} are not saved here. Instead sm_fiqhandler
+ * preserves r{8-12}.
+ */
+
+ uint32_t svc_spsr;
+ uint32_t svc_sp;
+ uint32_t svc_lr;
+ uint32_t abt_spsr;
+ uint32_t abt_sp;
+ uint32_t abt_lr;
+ uint32_t und_spsr;
+ uint32_t und_sp;
+ uint32_t und_lr;
+ uint32_t pmcr;
+};
+
+struct arm_sm_nsec_ctx
+{
+ struct arm_sm_banked_regs regs;
+
+ uint32_t r8;
+ uint32_t r9;
+ uint32_t r10;
+ uint32_t r11;
+ uint32_t r12;
+
+ uint32_t r0;
+ uint32_t r1;
+ uint32_t r2;
+ uint32_t r3;
+ uint32_t r4;
+ uint32_t r5;
+ uint32_t r6;
+ uint32_t r7;
+
+ /* Return state */
+
+ uint32_t mon_lr;
+ uint32_t mon_spsr;
+};
+
+struct arm_sm_sec_ctx
+{
+ struct arm_sm_banked_regs regs;
+
+ uint32_t r0;
+ uint32_t r1;
+ uint32_t r2;
+ uint32_t r3;
+ uint32_t r4;
+ uint32_t r5;
+ uint32_t r6;
+ uint32_t r7;
+
+ /* Return state */
+
+ uint32_t mon_lr;
+ uint32_t mon_spsr;
+};
+
+struct arm_sm_ctx
+{
+ struct arm_sm_sec_ctx sec;
+ struct arm_sm_nsec_ctx nsec;
+};
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: arm_sm_init
+ *
+ * Description:
+ * Initializes secure monitor
+ *
+ * Input Parameters:
+ * None
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void arm_sm_init(void);
+
+/****************************************************************************
+ * Name: arm_sm_init_stack
+ *
+ * Description:
+ * Initializes secure monitor
+ *
+ * Input Parameters:
+ * stack - the address of the stack that allocated for secure monitor
+ * runtime
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void arm_sm_init_stack(int stack);
+
+/****************************************************************************
+ * Name: arm_sm_boot_nsec
+ *
+ * Description:
+ * Set up the non-secure startup context, the entry will set to the
+ * non-secure context lr register, thus when we boot ap from tee, the
+ * non-secure will start running from the address that defined in lr
+ * register, of which contains the entry
+ *
+ * Input Parameters:
+ * entry - the entry address of the ap core
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void arm_sm_boot_nsec(uintptr_t entry);
+
+/****************************************************************************
+ * Name: arm_sm_get_nsec_ctx
+ *
+ * Description:
+ * Get the non-secure context that used by secure monitor mode
+ *
+ * Input Parameters:
+ * None
+ *
+ * Returned Value:
+ * The arm_sm_nsec_ctx of which contains the non-secure context
+ *
+ ****************************************************************************/
+
+struct arm_sm_nsec_ctx *arm_sm_get_nsec_ctx(void);
+
+/****************************************************************************
+ * Name: arm_sm_save_banked_regs
+ *
+ * Description:
+ * Save the banked registers set
+ *
+ * Input Parameters:
+ * arm_sm_banked_regs - the banked registers set
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void arm_sm_save_banked_regs(struct arm_sm_banked_regs *regs);
+
+/****************************************************************************
+ * Name: arm_sm_restore_banked_regs
+ *
+ * Description:
+ * To restore the banked registers that saved before
+ *
+ * Input Parameters:
+ * arm_sm_banked_regs - the banked registers set
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void arm_sm_restore_banked_regs(struct arm_sm_banked_regs *regs);
+
+/****************************************************************************
+ * Name: arm_vectorsmc
+ *
+ * Description:
+ * The arm_vectorsmc internal will make "smc" call to enter into the
+ * monitor mode
+ *
+ * Input Parameters:
+ * a0 - the r0, stored in register
+ * a1 - the r1, stored in register
+ * a2 - the r2, stored in register
+ * a3 - the r3, stored in register
+ * a4 - the r4, stored in stack
+ * a5 - the r5, stored in stack
+ * a6 - the r6, stored in stack
+ * a7 - the r7, stored in stack
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void arm_vectorsmc(uint32_t a0, uint32_t a1, uint32_t a2, uint32_t a3,
+ uint32_t a4, uint32_t a5, uint32_t a6, uint32_t a7);
+
+/****************************************************************************
+ * Name: arm_vectorfiq_entry
+ *
+ * Description:
+ * This function is called when non-secure world trigger FIQ, and trapped
+ * to monitor world, and then this FIQ is handled in secure world
+ *
+ * Input Parameters:
+ * a0 - the r0, stored in register
+ * a1 - the r1, stored in register
+ * a2 - the r2, stored in register
+ * a3 - the r3, stored in register
+ * a4 - the r4, stored in stack
+ * a5 - the r5, stored in stack
+ * a6 - the r6, stored in stack
+ * a7 - the r7, stored in stack
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void arm_vectorfiq_entry(uint32_t a0, uint32_t a1, uint32_t a2, uint32_t a3,
+ uint32_t a4, uint32_t a5, uint32_t a6, uint32_t a7);
+
+/****************************************************************************
+ * Name: arm_sm_switch_nsec
+ *
+ * Description:
+ * This function is using to switch from secure to non-secure, is called
+ * from up_idle() function. This function is implemented by make "smc" call
+ * internal.
+ *
+ * Input Parameters:
+ * None
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void arm_sm_switch_nsec(void);
+
+#endif /* __ARCH_ARM_SRC_ARMV7_A_SM_H */