2 files changed, 30 insertions, 27 deletions

diff --git a/include/macros.inc b/include/macros.inc
index 9eac27a..7d2f10f 100644
--- a/include/macros.inc
+++ b/include/macros.inc
@@ -15,3 +15,32 @@
 	cps #0x13 // Setup sp in SVC mode.
 	ldr sp, =svc_stack_core\coreid
 .endm
+
+.macro preserve_ctx
+	cps #0x1f // Sys mode
+	// Store Usr regs
+	push {r0-r12}
+	push {lr}
+	ldr r3, =scheduler // struct Scheduler
+	ldr r2, [r3, #0] // struct Thread* rthread
+	str sp, [r2, #4] // svc_lr -> void* sp
+	cps #0x13 // Svc mode
+	mrs r1, spsr
+	str r1, [r2, #0xc] // preserve cpsr
+	str lr, [r2, #0] // svc_lr -> void* pc
+.endm
+
+.macro restore_ctx
+	ldr r3, =scheduler // struct Scheduler
+	ldr r2, [r3, #0] // struct Thread* rthread
+	ldr lr, [r2, #0] // void* pc -> lr_svc
+	ldr r0, [r2, #4] // void* sp -> r0
+	ldr r1, [r2, #0xc] // restore cpsr
+	msr spsr_f, r1
+	cps #0x1f // Sys mode
+	mov sp, r0 // Set stack pointer
+	// Restore Usr regs
+	pop {lr}
+	pop {r0-r12}
+	cps #0x13 // Svc mode
+.endm
diff --git a/src/sys/schedule.S b/src/sys/schedule.S
index 3c49494..d10b844 100644
--- a/src/sys/schedule.S
+++ b/src/sys/schedule.S
@@ -1,33 +1,7 @@
 .section ".text"
 .globl schedule
-.macro preserve_ctx
-	cps #0x1f // Sys mode
-	// Store Usr regs
-	push {r0-r12}
-	push {lr}
-	ldr r3, =scheduler // struct Scheduler
-	ldr r2, [r3, #0] // struct Thread* rthread
-	str sp, [r2, #4] // svc_lr -> void* sp
-	cps #0x13 // Svc mode
-	mrs r1, spsr
-	str r1, [r2, #0xc] // preserve cpsr
-	str lr, [r2, #0] // svc_lr -> void* pc
-.endm
 
-.macro restore_ctx
-	ldr r3, =scheduler // struct Scheduler
-	ldr r2, [r3, #0] // struct Thread* rthread
-	ldr lr, [r2, #0] // void* pc -> lr_svc
-	ldr r0, [r2, #4] // void* sp -> r0
-	ldr r1, [r2, #0xc] // restore cpsr
-	msr spsr_f, r1
-	cps #0x1f // Sys mode
-	mov sp, r0 // Set stack pointer
-	// Restore Usr regs
-	pop {lr}
-	pop {r0-r12}
-	cps #0x13 // Svc mode
-.endm
+.include "macros.inc"
 
 // Assumption: Enter in SVC mode
 schedule: