From 32f56c3c626cc1bcd56a9c8e2c295e31ef44d6c9 Mon Sep 17 00:00:00 2001
From: ligd <liguiding1@xiaomi.com>
Date: Fri, 29 Mar 2024 20:55:15 +0800
Subject: [PATCH] armv8m: support busfault forward to TEE in REE handler mode

Signed-off-by: ligd <liguiding1@xiaomi.com>
---
 arch/arm/src/armv8-m/arm_gen_nonsecfault.c | 56 +++++++++++++++-------
 arch/arm/src/armv8-m/arm_securefault.c     |  7 ++-
 2 files changed, 43 insertions(+), 20 deletions(-)

diff --git a/arch/arm/src/armv8-m/arm_gen_nonsecfault.c b/arch/arm/src/armv8-m/arm_gen_nonsecfault.c
index f54fc92ad0..107063cba5 100644
--- a/arch/arm/src/armv8-m/arm_gen_nonsecfault.c
+++ b/arch/arm/src/armv8-m/arm_gen_nonsecfault.c
@@ -46,6 +46,17 @@
 #define OFFSET_R15             (6 * 4) /* R15 = PC */
 #define OFFSET_XPSR            (7 * 4) /* xPSR */
 
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static uint32_t g_psp_ns;
+static uint32_t g_msp_ns;
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
 /****************************************************************************
  * Name: arm_should_gen_nonsecurefault
  *
@@ -85,7 +96,7 @@ bool weak_function arm_should_gen_nonsecurefault(void)
 
 int arm_gen_nonsecurefault(int irq, uint32_t *regs)
 {
-  uint32_t nsp;
+  uint32_t sp_ns;
 
   if (!arm_should_gen_nonsecurefault())
     {
@@ -99,38 +110,51 @@ int arm_gen_nonsecurefault(int irq, uint32_t *regs)
       return 0;
     }
 
-  /* busfault are forward to REE ? */
-
-  if (getreg32(NVIC_AIRCR) & NVIC_AIRCR_BFHFNMINS)
+  if (getreg32(SAU_SFSR) == 0)
     {
-      return 0;
+      /* busfault are forward to REE ? */
+
+      if (getreg32(NVIC_AIRCR) & NVIC_AIRCR_BFHFNMINS)
+        {
+          return 0;
+        }
+
+      /* Redict busfault to REE */
+
+      up_secure_irq(NVIC_IRQ_BUSFAULT, false);
     }
 
-  /* Redict busfault to REE */
-
-  up_secure_irq(NVIC_IRQ_BUSFAULT, false);
-
   /* Get non-secure SP */
 
-  __asm__ __volatile__ ("mrs %0, msp_ns" : "=r" (nsp));
+  if (regs[REG_EXC_RETURN] & EXC_RETURN_THREAD_MODE)
+    {
+      __asm__ __volatile__ ("mrs %0, psp_ns" : "=r" (g_psp_ns));
+      sp_ns = g_psp_ns;
+    }
+  else
+    {
+      __asm__ __volatile__ ("mrs %0, msp_ns" : "=r" (g_msp_ns));
+      sp_ns = g_msp_ns;
+    }
 
   _alert("Dump REE registers:\n");
   _alert("R0: %08" PRIx32 " R1: %08" PRIx32
          " R2: %08" PRIx32 "  R3: %08" PRIx32 "\n",
-         getreg32(nsp + OFFSET_R0), getreg32(nsp + OFFSET_R1),
-         getreg32(nsp + OFFSET_R2), getreg32(nsp + OFFSET_R3));
+         getreg32(sp_ns + OFFSET_R0), getreg32(sp_ns + OFFSET_R1),
+         getreg32(sp_ns + OFFSET_R2), getreg32(sp_ns + OFFSET_R3));
   _alert("IP: %08" PRIx32 " SP: %08" PRIx32
           " LR: %08" PRIx32 "  PC: %08" PRIx32 "\n",
-         getreg32(nsp + OFFSET_R12), nsp,
-         getreg32(nsp + OFFSET_R14), getreg32(nsp + OFFSET_R15));
+         getreg32(sp_ns + OFFSET_R12), sp_ns,
+         getreg32(sp_ns + OFFSET_R14), getreg32(sp_ns + OFFSET_R15));
+
   syslog_flush();
 
   /* Force set return ReturnAddress to 0, then non-secure cpu will crash.
    * Also, the ReturnAddress is very important, so move it to R12.
    */
 
-  putreg32(getreg32(nsp + OFFSET_R15), nsp + OFFSET_R12);
-  putreg32(0, nsp + OFFSET_R15);
+  putreg32(getreg32(sp_ns + OFFSET_R15), sp_ns + OFFSET_R12);
+  putreg32(0, sp_ns + OFFSET_R15);
 
   return 1;
 }
diff --git a/arch/arm/src/armv8-m/arm_securefault.c b/arch/arm/src/armv8-m/arm_securefault.c
index 82285196eb..6b052138b8 100644
--- a/arch/arm/src/armv8-m/arm_securefault.c
+++ b/arch/arm/src/armv8-m/arm_securefault.c
@@ -112,17 +112,16 @@ int arm_securefault(int irq, void *context, void *arg)
       sfalert("\tLazy state error\n");
     }
 
-  /* clear SFSR sticky bits */
-
-  putreg32(0xff, SAU_SFSR);
-
 #ifdef CONFIG_DEBUG_SECUREFAULT
   if (arm_gen_nonsecurefault(irq, context))
     {
+      putreg32(0xff, SAU_SFSR);
       return OK;
     }
 #endif
 
+  putreg32(0xff, SAU_SFSR);
+
   up_irq_save();
   PANIC_WITH_REGS("panic", context);