WIP: Handle armv8 debug box entry correctly

core/debug/src/core_armv8m/debug_box_armv8m.c

@@ -19,6 +19,10 @@
 #include "context.h"
 #include "linker.h"
 #include "secure_transitions.h"
+#include "vmpu_mpu.h"
+#include "vmpu_unpriv_access.h"
+
+extern uint32_t g_debug_interrupt_sp[];
 
 void debug_die(void)
 {
@@ -29,15 +33,44 @@ void debug_die(void)
 void debug_deprivilege_and_return(void * debug_handler, void * return_handler,
                                   uint32_t a0, uint32_t a1, uint32_t a2, uint32_t a3)
 {
-    /* Switch to the debug box.
-     * We use a regular process switch, so we don't need a dedicated stack for
-     * the debug box. */
-    uint8_t box_id = g_debug_box.box_id;
-    context_switch_in(CONTEXT_SWITCH_FUNCTION_DEBUG, box_id, __TZ_get_SP_NS(), g_context_current_states[box_id].sp);
-
-    /* De-privilege, call the debug box handler, re-privilege, call the return
-     * handler. */
-    uint32_t caller = UVISOR_GET_NS_ALIAS(UVISOR_GET_NS_ADDRESS((uint32_t) debug_handler));
-    SECURE_TRANSITION_S_TO_NS(caller, a0, a1, a2, a3);
-    ((void (*)(void)) return_handler)();
+    /* Destination box: The debug box. */
+    uint8_t dst_id = g_debug_box.box_id;
+    if (__get_IPSR()) {
+        /* Source box: Get the current stack pointer. */
+        /* Note: The source stack pointer is only used to assess the stack
+        *       alignment and to read the xpsr. */
+        uint32_t src_sp = context_validate_exc_sf(__TZ_get_SP_NS());
+
+        /* FIXME: This makes the debug box overwrite the top of the interrupt stack! */
+        g_context_current_states[dst_id].sp = g_debug_interrupt_sp[dst_id];
+
+        /* Destination box: Forge the destination stack frame. */
+        /* Note: We manually have to set the 4 parameters on the destination stack,
+        *       so we will set the API to have nargs=0. */
+        uint32_t dst_sp = context_forge_exc_sf(src_sp, dst_id, (uint32_t) debug_handler, (uint32_t) return_handler, xPSR_T_Msk, 0);
+        ((uint32_t *) dst_sp)[0] = a0;
+        ((uint32_t *) dst_sp)[1] = a1;
+        ((uint32_t *) dst_sp)[2] = a2;
+        ((uint32_t *) dst_sp)[3] = a3;
+
+        /* Stop all lower-than-SVC-priority interrupts. FIXME Enable debug box to
+        * do things that require interrupts. One idea would be to provide an SVC
+        * to re-enable interrupts that can only be called by the debug box during
+        * debug handling. */
+        //__set_BASEPRI(__UVISOR_NVIC_MIN_PRIORITY << (8U - __NVIC_PRIO_BITS));
+
+        context_switch_in(CONTEXT_SWITCH_FUNCTION_DEBUG, dst_id, src_sp, dst_sp);
+    } else {
+        /* Switch to the debug box.
+        * We use a regular process switch, so we don't need a dedicated stack for
+        * the debug box. */
+
+        context_switch_in(CONTEXT_SWITCH_FUNCTION_DEBUG, dst_id, __TZ_get_SP_NS(), g_context_current_states[dst_id].sp);
+
+        /* De-privilege, call the debug box handler, re-privilege, call the return
+        * handler. */
+        uint32_t caller = UVISOR_GET_NS_ALIAS(UVISOR_GET_NS_ADDRESS((uint32_t) debug_handler));
+        SECURE_TRANSITION_S_TO_NS(caller, a0, a1, a2, a3);
+        ((void (*)(void)) return_handler)();
+    }
 }

core/vmpu/src/vmpu.c

@@ -380,7 +380,7 @@ static void vmpu_configure_box_sram(uint8_t box_id, UvisorBoxConfig const * box_
          *       architectures allow us to setup a background region somehow. */
         bss_start = (uint32_t) __uvisor_config.heap_start;
 #if defined (__ARM_FEATURE_CMSE) && (__ARM_FEATURE_CMSE == 3U)
-        stack_pointer = __TZ_get_PSP_NS();
+        stack_pointer = __TZ_get_SP_NS();
 #else
         stack_pointer = __get_PSP();
 #endif