diff --git a/qemu/target/arm/helper.c b/qemu/target/arm/helper.c
index 9f0a134d..45a25ed7 100644
--- a/qemu/target/arm/helper.c
+++ b/qemu/target/arm/helper.c
@@ -5318,26 +5318,39 @@ static bool v7m_using_psp(CPUARMState *env)
         env->v7m.control[env->v7m.secure] & R_V7M_CONTROL_SPSEL_MASK;
 }
 
+/* Write to v7M CONTROL.SPSEL bit for the specified security bank.
+ * This may change the current stack pointer between Main and Process
+ * stack pointers if it is done for the CONTROL register for the current
+ * security state.
+ */
+static void write_v7m_control_spsel_for_secstate(CPUARMState *env,
+                                                 bool new_spsel,
+                                                 bool secstate)
+{
+    bool old_is_psp = v7m_using_psp(env);
+
+    env->v7m.control[secstate] =
+        deposit32(env->v7m.control[secstate],
+                  R_V7M_CONTROL_SPSEL_SHIFT,
+                  R_V7M_CONTROL_SPSEL_LENGTH, new_spsel);
+
+    if (secstate == env->v7m.secure) {
+        bool new_is_psp = v7m_using_psp(env);
+        uint32_t tmp;
+        if (old_is_psp != new_is_psp) {
+            tmp = env->v7m.other_sp;
+            env->v7m.other_sp = env->regs[13];
+            env->regs[13] = tmp;
+        }
+    }
+}
+
 /* Write to v7M CONTROL.SPSEL bit. This may change the current
  * stack pointer between Main and Process stack pointers.
  */
 static void write_v7m_control_spsel(CPUARMState *env, bool new_spsel)
 {
-    uint32_t tmp;
-    bool new_is_psp, old_is_psp = v7m_using_psp(env);
-
-    env->v7m.control[env->v7m.secure] =
-        deposit32(env->v7m.control[env->v7m.secure],
-                  R_V7M_CONTROL_SPSEL_SHIFT,
-                  R_V7M_CONTROL_SPSEL_LENGTH, new_spsel);
-
-    new_is_psp = v7m_using_psp(env);
-
-    if (old_is_psp != new_is_psp) {
-        tmp = env->v7m.other_sp;
-        env->v7m.other_sp = env->regs[13];
-        env->regs[13] = tmp;
-    }
+    write_v7m_control_spsel_for_secstate(env, new_spsel, env->v7m.secure);
 }
 
 void write_v7m_exception(CPUARMState *env, uint32_t new_exc)
@@ -5535,6 +5548,7 @@ static void do_v7m_exception_exit(ARMCPU *cpu)
     bool return_to_sp_process = false;
     bool return_to_handler = false;
     bool rettobase = false;
+    bool exc_secure = false;
     bool return_to_secure;
 
     /* We can only get here from an EXCP_EXCEPTION_EXIT, and
@@ -5573,13 +5587,13 @@ static void do_v7m_exception_exit(ARMCPU *cpu)
          * value of the ES bit in the exception return value indicates
          * which security state's faultmask to clear. (v8M ARM ARM R_KBNF.)
          */
-        /* Unicorn: commented out
         if (arm_feature(env, ARM_FEATURE_M_SECURITY)) {
-            int es = excret & R_V7M_EXCRET_ES_MASK;
-            if (armv7m_nvic_raw_execution_priority(env->nvic) >= 0) {
-                env->v7m.faultmask[es] = 0;
-            }
-        } else*/ {
+            exc_secure = excret & R_V7M_EXCRET_ES_MASK;
+            // Unicorn: commented out
+            //if (armv7m_nvic_raw_execution_priority(env->nvic) >= 0) {
+            //    env->v7m.faultmask[es] = 0;
+            //}
+        } else {
             env->v7m.faultmask[M_REG_NS] = 0;
         }
     }
@@ -5643,7 +5657,7 @@ static void do_v7m_exception_exit(ARMCPU *cpu)
      * Handler mode (and will be until we write the new XPSR.Interrupt
      * field) this does not switch around the current stack pointer.
      */
-    write_v7m_control_spsel(env, return_to_sp_process);
+    write_v7m_control_spsel_for_secstate(env, return_to_sp_process, exc_secure);
 
     switch_v7m_security_state(env, return_to_secure);