diff options
| -rw-r--r-- | include/sys/schedule.h | 1 | ||||
| -rw-r--r-- | src/sys/schedule.S | 6 |
2 files changed, 6 insertions, 1 deletions
diff --git a/include/sys/schedule.h b/include/sys/schedule.h index 8a68e05..4d331a6 100644 --- a/include/sys/schedule.h +++ b/include/sys/schedule.h @@ -32,6 +32,7 @@ struct Thread { unsigned char priority; unsigned char preempt; unsigned short status; + unsigned long cpsr; }; struct ThreadRotBuffer { diff --git a/src/sys/schedule.S b/src/sys/schedule.S index c8dcaa2..aa55ac2 100644 --- a/src/sys/schedule.S +++ b/src/sys/schedule.S @@ -10,9 +10,11 @@ push {lr} ldr r3, =scheduler // struct Scheduler ldr r2, [r3, #0] // struct Thread* rthread - str lr, [r2, #0] // svc_lr -> void* pc str sp, [r2, #4] // svc_lr -> void* sp + mrs r1, cpsr + str r1, [r2, #0xc] // preserve cpsr cps #0x13 // Svc mode + str lr, [r2, #0] // svc_lr -> void* pc .endm .macro restore_ctx @@ -20,7 +22,9 @@ 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 cps #0x1f // Sys mode + msr cpsr, r1 mov sp, r0 // Set stack pointer // Restore Usr regs pop {lr} |