Recover from stacking errors

Patater · Patater · commit 5d0f159a2abf · 2016-11-14T15:59:26.000Z
We hard fault if we try to read from the stack if the mem manage fault is a
stacking or unstacking fault. Check the fault status before reading the pc
from the stack to avoid the hard fault.
diff --git a/core/debug/inc/debug.h b/core/debug/inc/debug.h
@@ -42,6 +42,13 @@ uint32_t debug_get_version(void);
 void debug_halt_error(THaltError reason);
 void debug_reboot(TResetReason reason);
 
+/* Enter the debug box from a privileged mode exception handler. This function
+ * requires the caller to have already switched the PSP to the debug box stack.
+ * We currently only call this on MPU faults and Hard Faults in
+ * vmpu_sys_mux_handler. If called from outside a privileged mode exception
+ * handler, this function does nothing. */
+uint32_t debug_box_enter_from_priv(uint32_t lr);
+
 #ifdef  NDEBUG
 
 #define DEBUG_INIT(...)          {}
diff --git a/core/debug/src/debug_box.c b/core/debug/src/debug_box.c
@@ -142,3 +142,31 @@ void debug_register_driver(const TUvisorDebugDriver * const driver)
     g_debug_box.box_id = g_active_box;
     g_debug_box.initialized = 1;
 }
+
+/* FIXME This is a bit platform specific. Consider moving to a platform
+ * specific location. */
+uint32_t debug_box_enter_from_priv(uint32_t lr) {
+    uint32_t shcsr;
+    uint32_t from_priv = !(lr & 0x4);
+
+    /* If we are not handling an exception caused from privileged mode, return
+     * the original lr. */
+    if (!from_priv) {
+        return lr;
+    }
+
+    shcsr = SCB->SHCSR;
+
+    /* Make sure SVC is active. */
+    assert(shcsr & SCB_SHCSR_SVCALLACT_Msk);
+
+    /* We had a fault (from SVC), so clear the SVC fault before returning. SVC
+     * and all other exceptions must be no longer active after the EXC RETURN,
+     * or else we cause usage faults when doing SVCs later (for example, to
+     * reboot via the debug_reboot SVC). */
+    SCB->SHCSR = shcsr & ~SCB_SHCSR_SVCALLACT_Msk;
+
+    /* Return to Thread mode and use the Process Stack for return. The PSP will
+     * have been changed already. */
+    return 0xFFFFFFFD;
+}
diff --git a/core/system/src/system.c b/core/system/src/system.c
@@ -91,9 +91,8 @@ void UVISOR_NAKED UVISOR_NORETURN isr_default_sys_handler(void)
     asm volatile(
         "mov r0, lr\n"
         "mrs r1, MSP\n"
-        "push {lr}\n"
-        "blx vmpu_sys_mux_handler\n"
-        "pop {pc}\n"
+        "bl vmpu_sys_mux_handler\n"
+        "bx r0\n"
     );
 }
 
diff --git a/core/vmpu/inc/vmpu.h b/core/vmpu/inc/vmpu.h
@@ -155,7 +155,9 @@ extern void vmpu_arch_init_hw(void);
 extern int  vmpu_init_pre(void);
 extern void vmpu_init_post(void);
 
-extern void vmpu_sys_mux_handler(uint32_t lr, uint32_t msp);
+/* Handle system exceptions and interrupts. Return the EXC_RETURN desired for
+ * returning from exception mode. */
+extern uint32_t vmpu_sys_mux_handler(uint32_t lr, uint32_t msp);
 
 /* contains the total number of boxes
  * boxes are enumerated from 0 to (g_vmpu_box_count - 1) and the following
diff --git a/core/vmpu/src/armv7m/vmpu_armv7m.c b/core/vmpu/src/armv7m/vmpu_armv7m.c
@@ -90,8 +90,11 @@ static int vmpu_fault_recovery_mpu(uint32_t pc, uint32_t sp, uint32_t fault_addr
     const MpuRegion *region;
     uint8_t mask, index, page;
 
-    /* No recovery is possible if the MPU syndrome register is not valid. */
-    if (fault_status == (SCB_CFSR_MMARVALID_Msk | SCB_CFSR_DACCVIOL_Msk)) {
+    /* No recovery is possible if the MPU syndrome register is not valid or
+     * this is not a stacking fault (where the MPU syndrome register would not
+     * be valid, but we can still recover). */
+    if (!((fault_status == (SCB_CFSR_MMARVALID_Msk | SCB_CFSR_DACCVIOL_Msk)) ||
+          (fault_status & (SCB_CFSR_MSTKERR_Msk | SCB_CFSR_MUNSTKERR_Msk)))) {
         return 0;
     }
 
@@ -112,7 +115,7 @@ static int vmpu_fault_recovery_mpu(uint32_t pc, uint32_t sp, uint32_t fault_addr
     return 1;
 }
 
-void vmpu_sys_mux_handler(uint32_t lr, uint32_t msp)
+uint32_t vmpu_sys_mux_handler(uint32_t lr, uint32_t msp)
 {
     uint32_t psp, pc;
     uint32_t fault_addr, fault_status;
@@ -127,29 +130,45 @@ void vmpu_sys_mux_handler(uint32_t lr, uint32_t msp)
 
     switch (ipsr) {
         case MemoryManagement_IRQn:
-            /* Currently we only support recovery from unprivileged mode. */
-            if (lr & 0x4) {
+            fault_status = VMPU_SCB_MMFSR;
+
+            /* If we are having an unstacking fault, we can't read the pc
+             * at fault. */
+            if (fault_status & (SCB_CFSR_MSTKERR_Msk | SCB_CFSR_MUNSTKERR_Msk)) {
+                /* Fake pc */
+                pc = 0x0;
+
+                /* The stack pointer is at fault. MMFAR doesn't contain a
+                 * valid fault address. */
+                fault_addr = lr & 0x4 ? psp : msp;
+            } else {
                 /* pc at fault */
-                pc = vmpu_unpriv_uint32_read(psp + (6 * 4));
+                if (lr & 0x4) {
+                    pc = vmpu_unpriv_uint32_read(psp + (6 * 4));
+                } else {
+                    /* We can be privileged here if we tried doing an ldrt or
+                     * strt to a region not currently loaded in the MPU. In
+                     * such cases, we are reading from the msp and shouldn't go
+                     * through vmpu_unpriv_uint32_read. A box wouldn't have
+                     * access to our stack. */
+                    pc = *(uint32_t *) (msp + (6 * 4));
+                }
 
                 /* Backup fault address and status */
                 fault_addr = SCB->MMFAR;
-                fault_status = VMPU_SCB_MMFSR;
-
-                /* Check if the fault is an MPU fault. */
-                if (vmpu_fault_recovery_mpu(pc, psp, fault_addr, fault_status)) {
-                    VMPU_SCB_MMFSR = fault_status;
-                    return;
-                }
+            }
 
-                /* If recovery was not successful, throw an error and halt. */
-                DEBUG_FAULT(FAULT_MEMMANAGE, lr, psp);
+            /* Check if the fault is an MPU fault. */
+            if (vmpu_fault_recovery_mpu(pc, psp, fault_addr, fault_status)) {
                 VMPU_SCB_MMFSR = fault_status;
-                HALT_ERROR(PERMISSION_DENIED, "Access to restricted resource denied");
-            } else {
-                DEBUG_FAULT(FAULT_MEMMANAGE, lr, msp);
-                HALT_ERROR(FAULT_MEMMANAGE, "Cannot recover from privileged MemManage fault");
+                return lr;
             }
+
+            /* If recovery was not successful, throw an error and halt. */
+            DEBUG_FAULT(FAULT_MEMMANAGE, lr, lr & 0x4 ? psp : msp);
+            VMPU_SCB_MMFSR = fault_status;
+            HALT_ERROR(PERMISSION_DENIED, "Access to restricted resource denied");
+            lr = debug_box_enter_from_priv(lr);
             break;
 
         case BusFault_IRQn:
@@ -173,16 +192,13 @@ void vmpu_sys_mux_handler(uint32_t lr, uint32_t msp)
                 /* Check if the fault is the special register corner case. */
                 if (!vmpu_fault_recovery_bus(pc, psp, fault_addr, fault_status)) {
                     VMPU_SCB_BFSR = fault_status;
-                    return;
+                    return lr;
                 }
-
-                /* If recovery was not successful, throw an error and halt. */
-                DEBUG_FAULT(FAULT_BUS, lr, psp);
-                HALT_ERROR(PERMISSION_DENIED, "Access to restricted resource denied");
-            } else {
-                DEBUG_FAULT(FAULT_BUS, lr, msp);
-                HALT_ERROR(FAULT_BUS, "Cannot recover from privileged bus fault");
             }
+
+            /* If recovery was not successful, throw an error and halt. */
+            DEBUG_FAULT(FAULT_BUS, lr, lr & 0x4 ? psp : msp);
+            HALT_ERROR(PERMISSION_DENIED, "Access to restricted resource denied");
             break;
 
         case UsageFault_IRQn:
@@ -193,6 +209,7 @@ void vmpu_sys_mux_handler(uint32_t lr, uint32_t msp)
         case HardFault_IRQn:
             DEBUG_FAULT(FAULT_HARD, lr, lr & 0x4 ? psp : msp);
             HALT_ERROR(FAULT_HARD, "Cannot recover from a hard fault.");
+            lr = debug_box_enter_from_priv(lr);
             break;
 
         case DebugMonitor_IRQn:
@@ -212,6 +229,8 @@ void vmpu_sys_mux_handler(uint32_t lr, uint32_t msp)
             HALT_ERROR(NOT_ALLOWED, "Active IRQn(%i) is not a system interrupt", ipsr);
             break;
     }
+
+    return lr;
 }
 
 static int vmpu_mem_push_page_acl_iterator(uint8_t mask, uint8_t index)
diff --git a/core/vmpu/src/kinetis/vmpu_kinetis.c b/core/vmpu/src/kinetis/vmpu_kinetis.c
@@ -50,7 +50,7 @@ static int vmpu_fault_recovery_mpu(uint32_t pc, uint32_t sp, uint32_t fault_addr
     return -1;
 }
 
-void vmpu_sys_mux_handler(uint32_t lr, uint32_t msp)
+uint32_t vmpu_sys_mux_handler(uint32_t lr, uint32_t msp)
 {
     uint32_t psp, pc;
     uint32_t fault_addr, fault_status;
@@ -82,51 +82,67 @@ void vmpu_sys_mux_handler(uint32_t lr, uint32_t msp)
             /* Note: All recovery functions update the stacked stack pointer so
              * that exception return points to the correct instruction. */
 
-            /* Currently we only support recovery from unprivileged mode. */
-            if (lr & 0x4) {
+            fault_status = VMPU_SCB_BFSR;
+
+            /* If we are having an unstacking fault, we can't read the pc
+             * at fault. */
+            if (fault_status & (SCB_CFSR_MSTKERR_Msk | SCB_CFSR_MUNSTKERR_Msk)) {
+                /* fake pc */
+                pc = 0x0;
+
+                /* The stack pointer is at fault. BFAR doesn't contain a
+                 * valid fault address. */
+                fault_addr = psp;
+            } else {
                 /* pc at fault */
-                pc = vmpu_unpriv_uint32_read(psp + (6 * 4));
+                if (lr & 0x4) {
+                    pc = vmpu_unpriv_uint32_read(psp + (6 * 4));
+                } else {
+                    /* We can be privileged here if we tried doing an ldrt or
+                     * strt to a region not currently loaded in the MPU. In
+                     * such cases, we are reading from the msp and shouldn't go
+                     * through vmpu_unpriv_uint32_read. A box wouldn't have
+                     * access to our stack. */
+                    pc = *(uint32_t *) (msp + (6 * 4));
+                }
 
                 /* backup fault address and status */
                 fault_addr = SCB->BFAR;
-                fault_status = VMPU_SCB_BFSR;
-
-                /* Check if the fault is an MPU fault. */
-                int slave_port = vmpu_fault_get_slave_port();
-                if (slave_port >= 0) {
-                    /* If the fault comes from the MPU module, we don't use the
-                     * bus fault syndrome register, but the MPU one. */
-                    fault_addr = MPU->SP[slave_port].EAR;
-
-                    /* Check if we can recover from the MPU fault. */
-                    if (!vmpu_fault_recovery_mpu(pc, psp, fault_addr)) {
-                        /* We clear the bus fault status anyway. */
-                        VMPU_SCB_BFSR = fault_status;
-
-                        /* We also clear the MPU fault status bit. */
-                        vmpu_fault_clear_slave_port(slave_port);
-
-                        /* Recover from the exception. */
-                        return;
-                    }
-                } else if (slave_port == VMPU_FAULT_MULTIPLE) {
-                    DPRINTF("Multiple MPU violations found.\r\n");
-                }
+            }
+
+            /* Check if the fault is an MPU fault. */
+            int slave_port = vmpu_fault_get_slave_port();
+            if (slave_port >= 0) {
+                /* If the fault comes from the MPU module, we don't use the
+                 * bus fault syndrome register, but the MPU one. */
+                fault_addr = MPU->SP[slave_port].EAR;
 
-                /* Check if the fault is the special register corner case. */
-                if (!vmpu_fault_recovery_bus(pc, psp, fault_addr, fault_status)) {
+                /* Check if we can recover from the MPU fault. */
+                if (!vmpu_fault_recovery_mpu(pc, psp, fault_addr)) {
+                    /* We clear the bus fault status anyway. */
                     VMPU_SCB_BFSR = fault_status;
-                    return;
+
+                    /* We also clear the MPU fault status bit. */
+                    vmpu_fault_clear_slave_port(slave_port);
+
+                    /* Recover from the exception. */
+                    return lr;
                 }
+            } else if (slave_port == VMPU_FAULT_MULTIPLE) {
+                DPRINTF("Multiple MPU violations found.\r\n");
+            }
 
-                /* If recovery was not successful, throw an error and halt. */
-                DEBUG_FAULT(FAULT_BUS, lr, psp);
+            /* Check if the fault is the special register corner case. */
+            if (!vmpu_fault_recovery_bus(pc, psp, fault_addr, fault_status)) {
                 VMPU_SCB_BFSR = fault_status;
-                HALT_ERROR(PERMISSION_DENIED, "Access to restricted resource denied");
-            } else {
-                DEBUG_FAULT(FAULT_BUS, lr, msp);
-                HALT_ERROR(FAULT_BUS, "Cannot recover from privileged bus fault");
+                return lr;
             }
+
+            /* If recovery was not successful, throw an error and halt. */
+            DEBUG_FAULT(FAULT_BUS, lr, lr & 0x4 ? psp : msp);
+            VMPU_SCB_BFSR = fault_status;
+            HALT_ERROR(PERMISSION_DENIED, "Access to restricted resource denied");
+            lr = debug_box_enter_from_priv(lr);
             break;
 
         case UsageFault_IRQn:
@@ -137,6 +153,7 @@ void vmpu_sys_mux_handler(uint32_t lr, uint32_t msp)
         case HardFault_IRQn:
             DEBUG_FAULT(FAULT_HARD, lr, lr & 0x4 ? psp : msp);
             HALT_ERROR(FAULT_HARD, "Cannot recover from a hard fault.");
+            lr = debug_box_enter_from_priv(lr);
             break;
 
         case DebugMonitor_IRQn:
@@ -156,6 +173,8 @@ void vmpu_sys_mux_handler(uint32_t lr, uint32_t msp)
             HALT_ERROR(NOT_ALLOWED, "Active IRQn(%i) is not a system interrupt", ipsr);
             break;
     }
+
+    return lr;
 }
 
 void vmpu_acl_stack(uint8_t box_id, uint32_t bss_size, uint32_t stack_size)
