use core::arch::global_asm;

// ExceptionEntry pushes LR and saves all of the task register state to
// a TaskRegisters struct on the stack. It is intended to be used as a
// primitive for pre-emptive multitasking.
global_asm!(r#"
    .macro ExceptionEntry
    cpsid i
    mov r0, lr
    movs r1, #0x0F       // Check which mode we just came from
    ands r0, r1
    cmp r0, #0xD        // if we came from something other than thread mode/process stack,
    bne 2f              // skip stacking the rest of the registers and move on

    // stack the registers
    push {{r4, r5, r6, r7}} // push r4-r7
    mov r0, r8
    mov r1, r9
    mov r2, r10
    mov r3, r11
    push {{r0, r1, r2, r3}} // push r8-r11
    mrs r0, PSP         // grab the process stack pointer
    push {{r0}}         // push PSP
    sub sp, #32         // reserve space for the next eight registers
    mov r1, sp          // copy the stack pointer
    ldm r0!, {{r4, r5, r6, r7}} // load r0-r3
    stm r1!, {{r4, r5, r6, r7}} // store r0-r3
    ldm r0!, {{r4, r5, r6, r7}} // load r12, lr, pc, xpsr
    stm r1!, {{r4, r5, r6, r7}} // save r12, lr, pc, xpsr
    mov r0, sp          // get address of TaskRegisters struct
    b 3f

    // Don't stack, provide None
2:  movs r0, #0
3:  push {{lr}}         // save LR
    .endm
"#);

// ExceptionExit is the reverse of ExceptionEntry. It restores all of
// the task registers and then exits the exception handler via the LR
// pushed to the stack earlier.
global_asm!(r#"
    .macro ExceptionExit
    pop {{r0}}          // restore LR
    mov lr, r0          // copy to LR
    movs r1, #0xF
    ands r0, r1
    cmp r0, #0xD        // check if we were using the process stack
    bne 2f              // and if not, skip unstacking

    ldr r0, [sp, #0x20] // load PSP from the TaskRegisters struct
    msr PSP, r0         // Store it back in its register
    pop {{r4, r5, r6, r7}}      // load r0-r3
    stm r0!, {{r4, r5, r6, r7}} // store r0-r3
    pop {{r4, r5, r6, r7}}      // load r12, lr, pc, xpsr
    stm r0!, {{r4, r5, r6, r7}} // store r12, lr, pc, xpsr
    add sp, #4          // skip the stack value we already got
    pop {{r4, r5, r6, r7}}
    mov r8, r4
    mov r9, r5
    mov r10, r6
    mov r11, r7
    pop {{r4, r5, r6, r7}}

2:  cpsie i
    bx lr
    .endm
"#);