diff --git a/assembleur.s b/assembleur.s
index b17deb7842d58447b843b2debf0349d7a67e3989..7732ea669ad94bc6ab4ae69ac62a783fa6f6fdc9 100644
--- a/assembleur.s
+++ b/assembleur.s
@@ -9,30 +9,24 @@
 .global reset_user_stack
 .global switch_to_user_mode
 .global switch_to_supervisor_mode
+.global switch_mode
 .global change_ret
 .extern user_stack DATA				// this adress can be read like: ldr Rx, =user_stack
 
 reset_user_stack:
 	ldr r0, =user_stack
-	add r0, r0, #8192 				// 2048 * word_size
+	mov r1, USER_STACK_SIZE
+	lsl r1, r1, #2			// (e.g. multiply by word size == 4 bytes)
+	add r0, r1
 	msr PSP, r0
-	mov r1, #3
-	msr CONTROL, r1
-	isb
+	mov r2, #3
+	msr CONTROL, r2
 	bx  lr
 
-switch_to_user_mode:
-	mrs r0, control
-	mov r0, #3
-	msr control, r0
-	isb
-	bx lr
-
-switch_to_supervisor_mode:
-	mrs r0, control
-	mov r0, #0
-	msr control, r0
-	isb
+switch_mode:
+	mrs r1, control
+	mov r1, r0
+	msr control, r1
 	bx lr
 
 /*************************/