1 files changed, 12 insertions, 2 deletions
diff --git a/src/kern/priv.c b/src/kern/priv.c
index 3162639..2c45eca 100644
--- a/src/kern/priv.c
+++ b/src/kern/priv.c
@@ -2,6 +2,7 @@
 
 #include "arch.h"
 #include "kern/log.h"
+#include "kern/mem.h"
 
 void set_control_register(uint32_t reg)
 {
@@ -24,7 +25,16 @@ void enter_user_mode()
 
 void jump_to_user_mode()
 {
+  void* new_stack = kalloc(4096);
+  new_stack += 4096;
+
   asm volatile(
-      "mov r0, #1\n\t"
-      "msr control, r0\n\t");
+      "mov  r0, %0\n\t"
+      "msr  psp, r0\n\t"
+      "mrs  r0, control\n\t"
+      "orrs r0, r0, #3\n\t"
+      "msr  control, r0\n\t"
+      "isb\n\t"
+      "dsb\n\t"
+      "b usermode_start\n\t" : : "r"(new_stack));
 }