// 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 ctl_private_switch_context        
        .global ctl_private_switch_isr_context
        .global ctl_private_isr_return

/*
      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

#ifdef __ARCH_V3
        mov r1, #0x13       // SVC mode
#else
        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
#endif
        str r1, [r0]        // save cpsr        
        
        ldmfd r13!, {r1, r2}
        str r0, [r1]        // t->stackPointer=r0
#ifdef __ARCH_V3
        mov pc, lr
#else 
        bx lr
#endif

// 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]             
        // go to svc mode to restore registers
        mrs r0, cpsr
        bic r0, r0, #0x1F
        orr r0, r0, #0x13 /* Supervisor mode */
        msr cpsr_cxsf, r0
        // 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
l2: 
        ldmdb r11, {r11, sp-lr}
#ifdef __ARCH_V3
        mov pc, lr
#else 
        bx lr
#endif

// 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;

