.extern RDY_TASK
.extern CURRENT_TASK
.global start_multitasking
.global task_switch
.global enable_interrupts
.global disable_interrupts
.global critical_enter
.global critical_exit

.macro CTX_LOAD
    mov r0, #0xd3
    msr CPSR_c, r0

    ldr r0, =RDY_TASK
    ldr r0, [r0]
    ldmia r0!, {r12, r14}
    msr SPSR_fsxc, r12

    ldmia r0, {r0-r14}^
    nop
    movs pc, r14
.endm

.thumb
.global start_multitasking_from_thumb
.thumb_func
start_multitasking_from_thumb:
    bx pc
    nop
.arm

start_multitasking:
    ldr r0, =CURRENT_TASK
    ldr r1, =RDY_TASK
    ldr r1, [r1]
    str r1, [r0]

    CTX_LOAD

.thumb
.global task_switch_from_thumb
.thumb_func
task_switch_from_thumb:
    bx pc
    nop
.arm
task_switch:
    mrs r1, cpsr

    ldr r0, =CURRENT_TASK
    ldr r0, [r0]

    ldr r2, =task_switch_restart

    stmia r0!, {r1, r2}
    stmia r0!, {r0-r14}

    ldr r0, =CURRENT_TASK
    ldr r1, =RDY_TASK
    ldr r1, [r1]
    str r1, [r0]

    mov r0, #0xd3
    msr CPSR_c, r0

    ldr r0, =RDY_TASK
    ldr r0, [r0]
    ldmia r0!, {r12, r14}
    msr SPSR_fsxc, r12

    ldmia r0, {r0-r14}^
    nop
    movs pc, r14
task_switch_restart:
    bx lr


.thumb
.global critical_enter_from_thumb
.thumb_func
critical_enter_from_thumb:
    bx pc
    nop
.arm
critical_enter:
    mrs r0, cpsr
    mov r1, r0
    orr r1, r1, #0xc0 // set IF, disabling interrupts
    msr CPSR_c, r1
    bx lr

.thumb
.global critical_exit_from_thumb
.thumb_func
critical_exit_from_thumb:
    bx pc
    nop
.arm
critical_exit:
    msr CPSR_c, r0 // load CPSR_c from argument
    bx lr

enable_interrupts:
    mrs r0, cpsr
    bic r0, r0, #0xc0 // clear IF
    msr CPSR_c, r0
    bx lr

disable_interrupts:
    mrs r0, cpsr
    orr r0, r0, #0xc0 // set IF
    msr CPSR_c, r0
    bx lr

