V7a mon

arch/arm/src/armv7-a/Kconfig

@@ -220,3 +220,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.

arch/arm/src/armv7-a/Make.defs

@@ -67,6 +67,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

arch/arm/src/armv7-a/arm_cpuhead.S

@@ -430,6 +430,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 */

arch/arm/src/armv7-a/arm_head.S

@@ -680,6 +680,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().
 	 */

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;
+}