diff --git a/arch/at91sam3x8e/handle_svc.S b/arch/at91sam3x8e/handle_svc.S
index 5b3677d..9ad3811 100644
--- a/arch/at91sam3x8e/handle_svc.S
+++ b/arch/at91sam3x8e/handle_svc.S
@@ -4,48 +4,16 @@
 
 .text
 
-/* int arch_enter(void *sp); */
+/* void arch_enter(struct exc_context *context); */
 .extern arch_enter
 
-/* void *sched_switch(void *sp); */
-.extern sched_switch
-
 /* void handle_svc(void); */
 func_begin handle_svc
-	/*
-	 * Syscalls on Cortex-M use the following parameter calling convention:
-	 *
-	 * number: r7
-	 * rval: r0
-	 * arg1: r0
-	 * arg2: r1
-	 * arg3: r2
-	 * arg4: r3
-	 * arg5: r4
-	 * arg6: r5
-	 */
-
-	/*
-	 * like in irq_pend_sv, we save everything on the stack to make early
-	 * process switching possible in case the syscall is blocking.
-	 */
-	push	{r4-r11,lr}
-
-	mov	r0,	sp
-	bl	arch_enter		/* int need_resched = arch_enter(sp); */
-
-	cmp	r0,	#0
-	beq	svc_out
 
+	prepare_entry
 	mov	r0,	sp
-	bl	sched_switch		/* sp = sched_switch(sp); */
-	mov	sp,	r0
-
-	clrex
-
-svc_out:
-	pop	{r4-r11,lr}
-
+	bl	arch_enter		/* arch_enter(sp); */
+	prepare_leave
 	bx	lr
 
 func_end handle_svc
diff --git a/arch/at91sam3x8e/include/asm.S b/arch/at91sam3x8e/include/asm.S
index 12ca973..f0fd5e3 100644
--- a/arch/at91sam3x8e/include/asm.S
+++ b/arch/at91sam3x8e/include/asm.S
@@ -14,6 +14,47 @@
 	.size \name, .-\name
 .endm
 
+.macro prepare_entry
+	/*
+	 * Hardware automatically saves r0-r3, r12, pc, lr, and psr onto the
+	 * stack that was used before exception entry.  Bit 2 in lr is 1 if the
+	 * psp was used, and 0 if msp was used.  We store the appropriate stack
+	 * pointer into r12 and push it onto the stack
+	 */
+	tst	lr,	#(1 << 2)
+	ite	ne
+	mrsne	r12,	psp
+	moveq	r12,	sp
+
+	/*
+	 * The kernel doesn't like to be interrupted.
+	 * We don't wanna be impolite and hurt its feelings.
+	 */
+	cpsid	i
+
+	/*
+	 * This push matches struct exc_context, r12 is the old stack pointer
+	 * (which conveniently points to the remaining registers that were
+	 * saved by hardware, including the original value of r12 before entry).
+	 */
+	push	{r4-r12,lr}
+.endm
+
+.macro prepare_leave
+	pop	{r4-r12,lr}
+
+	clrex
+
+	/* Turn interrupts back on */
+	cpsie	i
+
+	tst	lr,	#(1 << 2)
+	ite	ne
+	msrne	psp,	r12
+	moveq	sp,	r12
+
+.endm prepare_leave
+
 /*
  * This file is part of Ardix.
  * Copyright (c) 2021 Felix Kopp <owo@fef.moe>.
diff --git a/arch/at91sam3x8e/sys.c b/arch/at91sam3x8e/sys.c
index fc2d858..734a124 100644
--- a/arch/at91sam3x8e/sys.c
+++ b/arch/at91sam3x8e/sys.c
@@ -28,7 +28,7 @@
  * So, since we trust the clock to be configured correctly before this global
  * variable is accessed anywhere, we initialize it to the 84 MHz clock.
  */
-uint32_t SystemCoreClock = 84000000UL;
+volatile uint32_t SystemCoreClock = 84000000UL;
 
 void sys_init(void)
 {
diff --git a/include/config.h.in b/include/config.h.in
index b07b93f..92e61d0 100644
--- a/include/config.h.in
+++ b/include/config.h.in
@@ -25,6 +25,7 @@
 #define CONFIG_SERIAL_BAUD @CONFIG_SERIAL_BAUD@
 #define CONFIG_SERIAL_BUFSZ @CONFIG_SERIAL_BUFSZ@
 #define CONFIG_PRINTF_BUFSZ @CONFIG_PRINTF_BUFSZ@
+#define CONFIG_IOMEM_SIZE @CONFIG_IOMEM_SIZE@
 
 /*
  * This file is part of Ardix.