2 files changed, 13 insertions, 15 deletions
diff --git a/src/kern/main.c b/src/kern/main.c
index 3af8beb..4fddb0a 100644
--- a/src/kern/main.c
+++ b/src/kern/main.c
@@ -35,19 +35,7 @@ void configure_mpu()
 int main()
 {
   configure_mpu();
-
-  klogf("Outside of usermode, I can still log stuff using klogf()\n");
-
-  enter_user_mode();
-
-  logs("Now that I'm in user mode, I have to log stuff using a system call\n");
-  logs(
-      "because I no longer have direct accss to USART2 and cannot use\n"
-      "klogf()\n");
-
-
-  for (;;)
-    ;
+  jump_to_user_mode();
 }
 
 #endif
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));
 }