// CrossWorks Tasking Library.
//
// Copyright (c) 2004 Rowley Associates Limited.
//
// This file may be distributed under the terms of the License Agreement
// provided with this software.
//
// THIS FILE IS PROVIDED AS IS WITH NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.

	.text
	.code 32
	.align 0

        .global ctl_private_init_registers
        .global swi_handler
	.global ctl_private_switch_context        
        .global ctl_private_switch_isr_context
        .global ctl_private_isr_return
        .global ctl_global_interrupts_enable
        .global ctl_global_interrupts_disable
	.global ctl_global_interrupts_set
        .global ctl_global_interrupts_re_enable_from_isr
        .global ctl_global_interrupts_un_re_enable_from_isr

/*
      user stack   
      r15 (pc) - higher address
      r14 (lr)   
      r12
      ....
      r0
  sp->cpsr      
*/

// void ctl_private_init_registers(Task *t, 
//                            void *(*entrypoint)(void *),
//                            void *parameter,                           
//                            void (*exitpoint)(void))
ctl_private_init_registers:
        stmfd r13!, {r0,r1} // save t and entrypoint to stack
        ldr r0, [r0]        // r0=t->stackPointer
        sub r0, r0, #4      // decrement before
        str r1, [r0]        // save starting pc
        sub r0, r0, #4
        str r3, [r0]        // save return address
        sub r0, r0, #4
        // save r12-r1
        mov r1, #12
l0:
        str r1, [r0]
        sub r0, r0, #4
        sub r1, r1, #1
        cmp r1, #0
        bne l0

        str r2, [r0]        // save parameter to r0
        sub r0, r0, #4

        ldr r1, [r13, #4]   // load saved entrypoint to r1
        tst r1, #0x1        // test the bottom bit
        movne r1, #0x3f     // THUMB in system mode
        moveq r1, #0x1f     // ARM in system mode
        str r1, [r0]        // save cpsr        
        
        ldmfd r13!, {r1, r2}
        str r0, [r1]        // t->stackPointer=r0

        bx lr

// this is used to enable pc and cpsr to be updated atomically
// when called r1 points to the register state to be restored
swi_handler:  
        // ensure it is our swi        
        ldr r13, =SWIlabel+4 
        cmp r13, r14
        bne swi_handler
         
        // update spsr
        ldr r0, [r1]      // r0 = psr
        bic r0, r0, #0x80 // ensure that interrupts are enabled
        msr spsr, r0      // update spsr
        // update user r13
        add r0, r1, #16*4 // calculate user mode stack pointer value
        str r0, [r1]      // overwrite the saved psr value with it
        ldmfd r1, {r13}^  // load it into the user mode sp
        nop
        // restore the rest of the registers and user mode r14
        add r14, r1, #4
        ldmfd r14, {r0-r12,r14}^        
        nop
        // restore the pc and copy spsr to the cpsr
        ldr r14, [r14, #14*4]
        movs r15, r14

// void ctl_private_switch_context(Task *save, Task *restore);
ctl_private_switch_context:         
        // keep the debugger call stack unwinder happy
        mov   r12, sp
        stmfd r13!, {r11-r12, lr-pc}
        sub   r11, r12, #4

l1:  
        // save PC to be l2
        add   r3, r15, #l2-l1-8  
        stmfd r13!, {r3}
        // save other registers 
        stmfd r13!, {r0-r12,r14}   
        // save cpsr
        mrs r3, cpsr             
        stmfd r13!, {r3}
        // *save = r13
        stmia r0, {r13}          
        // r1 = restore->stack_pointer
        ldr r1, [r1]             
SWIlabel:
        swi 0x42                 // go to svc mode to restore registers
l2: 
        ldmdb r11, {r11, sp-lr}
        bx lr

// void ctl_private_switch_isr_context(void *savedRegisters, Task *save, Task *restore);
ctl_private_switch_isr_context:
        // save the parameters to the IRQ stack
        stmfd r13!, {r1,r2}
        // store the user mode stack pointer and link register
        mov r4, r13
        stmfd r4, {r13, r14}^
        ldr r5, [r4,#-4]               // r5 contains the user r14
        ldr r14, [r4,#-8]              // r14 contains the user r13        
        // store the saved lr-4 to *r4--
        ldr r2, [r0, #(4*14)]
        sub r2, r2, #4
        stmfd r14!, {r2}
        // store the user_r14 to *r4--
        stmfd r14!, {r5}
        // store r12 to *r4--
        ldr r2, [r0, #(4*13)]
        stmfd r14!, {r2}
        // load saved r0-r11 into r1 to r12
        ldmib r0, {r1-r12}
        // store saved r0-r11 into *r4-=11
        stmfd r14!, {r1-r12}
        // store saved spsr
        ldr r2, [r0]
        stmfd r14!, {r2}        
        
        // get the parameters back
        ldmfd r13!, {r1,r2}
        stmia r1, {r14}            // *save = r14

        // Clear up the IRQ stack
        add r13, r0, #4*15  // push the stack pointer to the start of the IRQ

        // Restore the user state from restore->stackPointer
        ldr r0, [r2]         // r0=restore->stackPointer

        // restore the spsr from saved
        ldmfd r0!, {r1}
        msr   spsr, r1

        // restore the user_lr and user_sp
        add    r2, r0, #4*15  // r2==user_sp value
        ldr    r3, [r2, #-8]  // r3==user_lr value
        stmfd r13!, {r2, r3}
        ldmfd r13, {r13, r14}^
        nop
        add   r13, r13, #8
        
        // restore r0-r12 from saved
        mov r14, r0
        ldmfd r14!, {r0-r12}

        // restore pc from saved
        ldr  r14, [r14, #4]
        movs r15, r14

// void ctl_private_isr_return(void *savedRegisters)
ctl_private_isr_return:
        mov sp, r0
        ldmfd sp!, {r0}
        msr spsr, r0
        ldmfd sp!, {r0-r12, lr}
        subs pc, lr, #4;

// int ctl_global_interrupts_enable()
ctl_global_interrupts_enable:
        mrs r3, cpsr
        and r2, r3, #0x80
        cmp r2, #0x0
        beq L0
        bic r3, r3, #0x80
        msr cpsr_cxsf, r3
        mov r0, #0   
        bx lr
L0:
        mov r0, #1  
        bx lr

// int ctl_global_interrupts_disable()
ctl_global_interrupts_disable:
        mrs r3, cpsr
        and r2, r3, #0x80
        cmp r2, #0x0
        bne L1
        orr r3, r3, #0x80
        msr cpsr_cxsf, r3
        mov r0, #1  
        bx lr
L1:
        mov r0, #0  
        bx lr

//void ctl_global_interrupts_set(int e)
ctl_global_interrupts_set:
        mrs r3, cpsr
        cmp r0, #0
        beq L2
        bic r3, r3, #0x80
        msr cpsr_cxsf, r3
        bx lr
L2:
        orr r3, r3, #0x80
        msr cpsr_cxsf, r3  
        bx lr

//void ctl_global_interrupts_re_enable_from_isr()
ctl_global_interrupts_re_enable_from_isr:
        /* Preserve the return address through mode change */
        mov r1, lr
        /* Enter System mode and re-enable IRQ interrupts */
        mrs r0, cpsr
        and r0, r0, #0x40
        orr r0, r0, #0x1F
        msr cpsr_c, r0
        /* Store lr_sys */
        stmfd sp!, {lr}
        /* Return to pre-mode change return address */
        bx r1

//void ctl_global_interrupts_un_re_enable_from_isr()
ctl_global_interrupts_un_re_enable_from_isr:
        /* Preserve the return address through mode change */
        mov r1, lr
        /* Restore lr_sys */
        ldmfd sp!, {lr}
        /* Enter IRQ mode and disable IRQ interrupts */
        mrs r0, cpsr
        and r0, r0, #0x40
        orr r0, r0, #0x92
        msr cpsr_c, r0
        /* Return to pre-mode change return address */
        bx r1
