Moved scheduler macros

1 files changed, 29 insertions, 0 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