debug and deprivilege mods for v8

Author: Oren Cohen

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,48 @@ 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));
+    /* 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());
+
+    /* Destination box: The debug box. */
+    uint8_t dst_id = g_debug_box.box_id;
+
+    /* Copy the xPSR from the source exception stack frame. */
+    uint32_t * xpsr_p = &((uint32_t *) src_sp)[7];
+    uint32_t xpsr = xPSR_T_Msk;
+    if (vmpu_buffer_access_is_ok(g_active_box, xpsr_p, sizeof(*xpsr_p))) {
+        xpsr = vmpu_unpriv_uint32_read((uint32_t) xpsr_p);
+    }
+
+    /* 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;
+
+    /* Suspend the OS. */
+    //g_priv_sys_hooks.priv_os_suspend();
+
+    /* 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);
+
+    /* Upon execution return debug_handler will be executed. Upon return from
+    * debug_handler, return_handler will be executed. */
+
+    //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

@@ -376,7 +376,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