Subversion Repositories c0or1k

/*
 * The vectors page. Includes all exception handlers.
 *
 * Copyright (C) 2007 Bahadir Balban
 */

#include INC_ARCH(asm.h)
#include INC_ARCH(asm-macros.S)

.section .data.vectors
__vector_vaddr:

BEGIN_PROC(arm_high_vector)
        b       arm_reset_exception
        b       arm_undef_exception_reentrant
        b       arm_swi_exception
        b       arm_prefetch_abort_exception_reentrant
        b       arm_data_abort_exception_reentrant
        nop
        b       arm_irq_exception_reentrant_with_schedule
        b       arm_fiq_exception
END_PROC(arm_high_vector)

.balign 4

/*
 * vect_reset
 *
 * Upon Entry:
 * - All registers are undefined and insignificant,
 * - FIQ/IRQs are disabled.
 * - PC:        0x00000000
 *
 *
 * PURPOSE:
 * CPU always starts executing from this vector
 * upon a HW reset. It may also be used as a SW reset.
 */
BEGIN_PROC(arm_reset_exception)
END_PROC(arm_reset_exception)


#if defined(CONFIG_SUBARCH_V5)
        .macro disable_irqs rx
                mrs     \rx, cpsr_fc
                orr     \rx, #ARM_IRQ_BIT
                msr     cpsr_fc, \rx
        .endm
        .macro enable_irqs rx
                mrs     \rx, cpsr_fc
                bic     \rx, #ARM_IRQ_BIT
                msr     cpsr_fc, \rx
        .endm
#endif

#if defined (CONFIG_SUBARCH_V7) || defined(CONFIG_SUBARCH_V6)
        .macro disable_irqs rx
                cpsid   ia
        .endm
        .macro enable_irqs rx
                cpsie   ia
        .endm
#endif

#if defined (CONFIG_SUBARCH_V7)
        .macro clear_exclusive
                clrex
        .endm
#else
        .macro clear_exclusive
        .endm
#endif

        /* Only works in SVC MODE. Know what you are doing! */
        .macro get_current rx
                bic     \rx, sp, #0xFF0
                bic     \rx, \rx, #0xF
        .endm
        /* Saves the address of system call argument registers pushed to stack
         * to the current task's ktcb. */
        .macro  ktcb_ref_saved_regs regs_addr, ktcb, regs_off
                get_current \ktcb
                ldr     \regs_off, =syscall_regs_offset
                ldr     \regs_off, [\regs_off]
                str     \regs_addr, [\ktcb, \regs_off]
        .endm
        /* Depending on the SPSR condition determines whether irqs should be enabled
         * during abort handling. If abort occured in userspace it orders irqs
         * should be enabled. Else if irqs come from kernel mode, it orders irqs are
         * enabled only if they were alreday enabled before the abort. */
        .macro  can_abort_enable_irqs temp1, r_spsr
                and \temp1, \r_spsr, #ARM_MODE_MASK
                cmp \temp1, #ARM_MODE_USR        @ Usermode indicates irqs can be enabled.
                beq 1f                      @ Z flag set. Which indicates "can enable"
                and \temp1, \r_spsr, #ARM_IRQ_BIT @ Clear irq bit indicates irqs were enabled
                cmp \temp1, #0              @ before the abort and can be safely enabled.
        1:                                  @ Z flag must be set for "can enable" here.
        .endm

        /* Pushes the user sp and lr to stack, updates the stack pointer */
        .macro push_user_sp_lr sp
                @ stack state: (Low) |..|..|->(Original)| (High)
                stmfd   \sp, {sp, lr}^  @ Push USR banked regs to stack.
                nop                     @ Need a NOOP after push/popping user registers.
                @ stack state: (Low) |SP_USR|LR_USR|->(Original)| (High)
                sub     \sp, \sp, #8    @ Adjust SP, since stack op on banked regs is no writeback.
                @ stack state: (Low) |->SP_USR|LR_USR|(Original)| (High)
        .endm

        .macro is_psr_usr rx
                and     \rx, \rx, #ARM_MODE_MASK
                cmp     \rx, #ARM_MODE_USR
        .endm

/* These really both read the same unified FSR and FAR registers */
#if defined (CONFIG_SUBARCH_V5)
        .macro  cp15_read_ifsr rx
                mrc     p15, 0, \rx, c5, c0, 0  @ Read FSR (Tells why the fault occured)

        .endm
        .macro cp15_read_ifar rx
                mrc     p15, 0, \rx, c6, c0, 0  @ Read FAR (Contains the faulted data address)
        .endm
        .macro  cp15_read_dfsr rx
                mrc     p15, 0, \rx, c5, c0, 0  @ Read FSR (Tells why the fault occured)

        .endm
        .macro cp15_read_dfar rx
                mrc     p15, 0, \rx, c6, c0, 0  @ Read FAR (Contains the faulted data address)
        .endm
#endif

/* These read the distinguished IFSR, IFAR, DFSR and DFAR registers */
#if defined (CONFIG_SUBARCH_V6) || defined (CONFIG_SUBARCH_V7)
        .macro  cp15_read_ifsr rx
                mrc     p15, 0, \rx, c5, c0, 1  @ Read IFSR (Tells why the fault occured)

        .endm
        .macro cp15_read_ifar rx
                mrc     p15, 0, \rx, c6, c0, 2  @ Read IFAR (Contains the faulted data address)
        .endm
        .macro  cp15_read_dfsr rx
                mrc     p15, 0, \rx, c5, c0, 0  @ Read DFSR (Tells why the fault occured)

        .endm
        .macro cp15_read_dfar rx
                mrc     p15, 0, \rx, c6, c0, 0  @ Read DFAR (Contains the faulted data address)
        .endm
#endif

#define UNDEF_R0        0
#define UNDEF_SPSR      -4
#define UNDEF_R14       -8

/*
 * vect_undef
 *
 * Upon Entry:
 * - R14:       Address of next instruction after undefined instruction
 * - PC:        0x00000004
 * - IRQs are disabled (CPSR[7] = 1)
 *
 *
 * PURPOSE:
 * A co-processor instruction not supported by the core can be
 * emulated here. Also unrecognised/invalid instructions are handled.
 */
BEGIN_PROC(arm_undef_exception_reentrant)
        clear_exclusive
        str     lr, [sp, #UNDEF_R14]    @ Store undef address
        mrs     lr, spsr                @ Get SPSR
        str     lr, [sp, #UNDEF_SPSR]   @ Store SPSR
        str     r0, [sp, #UNDEF_R0]     @ Store r0
        @ NOTE: Can increase undef nest here.
        mov     r0, sp                  @ Keep current sp point in R0
        mrs     lr, cpsr                @ Change to SVC mode.
        bic     lr, #ARM_MODE_MASK
        orr     lr, lr, #ARM_MODE_SVC
        msr     cpsr_fc, r14
        @ FIXME: Ensure 8-byte stack here.
        str     lr, [sp, #-8]!  @ Save lr_svc 2 words down from interrupted SP_SVC
        @ Transfer Undef state to SVC
        ldr     lr, [r0, #UNDEF_R14]
        str     lr, [sp, #4]
        @ Stack state:  |LR_SVC<-|LR_UNDEF|{original SP_SVC}|
        ldr     lr, [r0, #UNDEF_SPSR]
        ldr     r0, [r0, #UNDEF_R0]
        stmfd   sp!, {r0-r3,r12,lr}
        @ Stack state:  |R0<-|R1|R2|R3|R12|UNDEF_SPSR|LR_SVC|LR_DUNDEF|{original SP_SVC}|
        push_user_sp_lr sp      @ NOTE: These must be pushed to avoid trashing them if preempted
        @ Stack state: |SP_USR<-|LR_USR|R0<-|R1|R2|R3|R12|UNDEF_SPSR|LR_SVC|LR_DUNDEF|{original SP_SVC}|

        @ All undef state saved. Can safely enable irqs here, if need be.
        ldr     r3, [sp, #28]           @ Load UNDEF_SPSR
        can_abort_enable_irqs r0, r3    @ Judge if irqs can be enabled depending on prev state.
        bne     1f                      @ Branch here based on previous irq judgement.
        enable_irqs r3
1:
        /* Now check in what mode exception occured, and return that mode's LR in R4
         * Also poplulate r0,r1,r2 parameters for undefined_instr_handler
        */
        ldr     r1, [sp, #28]           @ Load UNDEF_SPSR
        is_psr_usr r0                   @ Test if UNDEF_SPSR was user mode.
        ldrne   r2, [sp, #32]           @ Abort occured in kernel, load LR_SVC
        ldreq   r2, [sp, #4]            @ Abort occured in user, load LR_USR
        ldr     r0, [sp, #36]           @ Load LR_UNDEF saved previously.
        mov     lr, pc
        ldr     pc, =undefined_instr_handler    @ Jump to function outside this page.
        disable_irqs r0                 @ Disable irqs to avoid corrupting spsr.
                                        @ (i.e. an interrupt could overwrite spsr with current psr)
        ldmfd   sp, {sp, lr}^           @ Restore user sp and lr which might have been corrupt on preemption
        nop                             @ User reg mod requires nop
        add     sp, sp, #8              @ Update SP.
        ldmfd   sp!, {r0-r3,r12,lr}     @ Restore previous context. (note, lr has spsr)
        msr     spsr_cxsf, r14          @ Restore spsr register from lr.
        @ Stack state: |LR_SVC<-|LR_PREV(UNDEF)|{original SP_SVC}|
        ldmfd   sp!, {r14, pc}^         @ Return, restoring cpsr. Note r14 gets r14_svc,
                                        @ and pc gets lr_undef. Saved at #4 and #8 offsets
                                        @ down from where svc stack had left.
END_PROC(arm_undef_exception_reentrant)

/*
 * vect_swi
 *
 * Upon Entry:
 * - R14:       Address of next instruction after the SWI
 * - PC:        0x00000008
 * - R0-R12:    Depending on the system call some of them contain
 *              indicators of what the exception means.
 * - IRQs are disabled (CPSR[7] = 1)
 * - SWI instruction's bits [7:0] may contain SWI indicator
 *
 * PURPOSE:
 * Used for trapping into a debugger or OS kernel via system calls.
 * Argument registers from R0 up to R12 and [7:0] of the causing SWI
 * instruction contains hints of what to do with this exception. What
 * R0-R12 contains depends on what userspace has put in them. Note this
 * is the only exception that userspace can generate and thus has control
 * on what it put into r0-rx.
 *
 * RECAP:
 * Normally across a function call, only r0-r3 are used for passing parameters.
 * Why r0-r3 only but not r4, r5...? See APCS (ARM procedure call standard)
 * Short answer: r4-r12 must be preserved across procedures but r0-r3 can be
 * trashed because they're set aside for argument passing. Arguments more than 4
 * go on the stack. Note APCS is a *suggestion*, rather than enforcement. So if
 * a userspace stub library is created that say, preserves and uses r0-r9 for a
 * system call, and the system call handler (this) knows about it, it is a
 * perfectly valid setup. In fact this is what we do here, we don't strictly use
 * r0-r3. Depending on the system call, the set of input registers (and output
 * registers to return results from the system call) may be redefined. These are
 * documented for each system call in the reference manual.
 * Another caveat to note in SWI usage is that we use the address offset of the
 * SWI instruction to see which offset it has in the system call vector, to
 * determine the correct system call, rather than [7:0] bits of the SWI.
 */
BEGIN_PROC(arm_swi_exception)
        clear_exclusive
        sub     lr, lr, #4      @ Get address of swi instruction user executed.
        stmfd   sp, {r0-r12,sp,lr}^ @ Push arguments, LR_USR and SP_USR to stack.
        nop

        @ Future optimisation 1:
        @ For all syscalls we need not push any more than r8 but we push up to
        @ r12 because upon a fork, a child's easiest way to restore user
        @ registers is to pop it from stack during return_from_syscall. In future
        @ fork function could return back to here, save all context into child
        @ from actual registers instead of reading from stack, and then return.

        @ Future optimisation 2:
        @ SP_USR MUST be pushed here, otherwise a kernel preemption could
        @ cause user mode of another process to overwrite SP_USR. The reason we
        @ save it here is because the preemption path does not currently save it
        @ if it is a kernel preemption. User SP can also be used here, as the
        @ user might have pushed data to its stack to be used by system calls.
        @ But we dont plan to pass data to kernel in this way, so saving of
        @ SP_USR can be done in preemption path as an optimisation.

        /*
         * The LR_usr is important here, because the user application uses a BL
         * to jump to the system call SWI, so the LR_usr contains the return
         * address, i.e. the next instruction after the *jumping* instruction to
         * the system call SWI (not the one after the swi itself, which is in
         * LR_svc).
         */

        sub     sp, sp, #60     @ stmfd on user registers can't writeback the SP. We do it manually.
        mrs     r0, spsr_fc     @ psr also need saving in case this context is interrupted.
        stmfd   sp!, {r0}
        enable_irqs r0
        mov     r0, sp          @ Current SP has pointer to all saved context.
        ktcb_ref_saved_regs r0, r1, r2 @ Save syscall context pointer in ktcb
        mov     r1, lr          @ Pass swi instruction address in LR as arg1
        mov     lr, pc
        ldr     pc, =syscall

.global return_from_syscall;    @ Newly created threads use this path to return,
return_from_syscall:            @ if they duplicated another thread's address space.
        disable_irqs r1         @ Not disabling irqs at this point causes the SP_USR and spsr
                                @ to get corrupt causing havoc.
        ldmfd   sp!, {r1}
        msr     spsr, r1
        add     sp, sp, #4      @ Skip, r0's location, since r0 already has returned result.
                                @ Note we're obliged to preserve at least r3-r8 because they're MRs.
        ldmfd   sp!, {r1-r12}   @ Restore r1-r8 pushed to stack earlier. r0 already has return result.
        ldmfd   sp, {sp}^       @ Restore user stack pointer, which might have been corrupt on preemption
        nop
        add     sp, sp, #4      @ Update sp.
        ldmfd   sp!, {lr}       @ Load userspace return address
        movs    pc, lr
END_PROC(arm_swi_exception)

/* Minimal abort state saved on data abort stack right after abort vector enters: */
#define ABT_R0          0
#define ABT_SPSR        -4
#define ABT_R14         -8

/* Minimal prefetch abort state saved on abort stack upon entry. */
#define ABT_R0          0
#define ABT_SPSR        -4
#define ABT_R14         -8

/*
 * vect_pabt
 *
 * Upon Entry:
 * - R14_abt:   Address of next instruction after aborted instruction
 * - R14_usr:   Address of return instruction in last function call**
 * - PC:        0x0000000c
 * - IRQs are disabled (CPSR[7] = 1)
 *
 *
 * PURPOSE:
 * Used for handling instructions that caused *memory aborts* during
 * the *prefetching* of the instruction. The instruction is also marked
 * as invalid by the core. It handles the cause for the memory abort.
 *
 * (One reason why a memory abort would occur is when we were entering
 * into a new page region that contained executable code and was not
 * present in memory, or its physical-to-virtual translation was not
 * present in the page tables. See other causes for memory aborts)
 *
 * **In case abort occured in userspace. This is useful if the abort
 * was due to a null/invalid function pointer call. Since R14_abt
 * includes the aborting instruction itself, R14_usr gives the clue to
 * where this call came from.
 */
BEGIN_PROC(arm_prefetch_abort_exception_reentrant)
        clear_exclusive
        sub     lr, lr, #4              @ lr-4 points at aborted instruction
        str     lr, [r13, #ABT_R14]     @ Store abort address.
        mrs     lr, spsr                @ Get SPSR
        str     lr, [r13, #ABT_SPSR]    @ Store SPSR
        str     r0, [r13, #ABT_R0]      @ Store R0 to use as temp register.
        mov     r0, r13                 @ SP to R0
        mrs     lr, cpsr                @ Change to SVC mode.
        bic     lr, #ARM_MODE_MASK
        orr     lr, lr, #ARM_MODE_SVC
        msr     cpsr_fc, r14
        @ FIXME: Ensure 8-byte stack here.
        str     lr, [sp, #-8]!  @ NOTE: Switched mode! Save LR_SVC 2 words down from SP_SVC.
transfer_pabt_state_to_svc:     @ Move data saved on PABT stack to SVC stack.
        ldr     lr, [r0, #ABT_R14]
        str     lr, [sp, #4]
        @ Stack state: |LR_SVC<-|LR_PABT|{original SP_SVC}|
        ldr     lr, [r0, #ABT_SPSR]
        ldr     r0, [r0, #ABT_R0]
        stmfd   sp!, {r0-r3,r12,lr}
        @ Stack state:  |R0<-|R1|R2|R3|R12|PABT_SPSR|LR_SVC|LR_PABT|{original SP_SVC}|
        push_user_sp_lr sp      @ NOTE: These must be pushed to avoid trashing if preempted
        @ Stack state:  |SP_USR<-|LR_USR|R0|R1|R2|R3|R12|PABT_SPSR|LR_SVC|LR_PABT|{original SP_SVC}|
read_pabt_state:
        cp15_read_ifsr r1       @ Reads FSR on ARMv5, IFSR on ARMv6-v7. Fault status information
        cp15_read_ifar r2       @ Reads FAR on ARMv5, IFAR on ARMv6-v7. Fault address information
        @ All abort state and (FAR/FSR) saved. Can safely enable irqs here, if need be.
        ldr     r3, [sp, #28]           @ Load PABT_SPSR
        can_abort_enable_irqs r0, r3    @ Judge if irqs can be enabled depending on prev state.
        bne     1f                      @ Branch here based on previous irq judgement.
        enable_irqs r3
1:
        ldr     r3, [sp, #28]           @ Load PABT_SPSR to r3, the spsr for the aborted mode
        ldr     r0, [sp, #36]           @ Load LR_PABT - 4 saved previously. (Address that aborted)
        mov     lr, pc
        ldr     pc, =prefetch_abort_handler @ Jump to function outside this page.
        disable_irqs r0                 @ Disable irqs to avoid corrupting spsr.
                                        @ (i.e. an interrupt could overwrite spsr with current psr)
        ldmfd   sp, {sp, lr}^           @ Restore user sp and lr which might have been corrupt on preemption
        nop                             @ User reg mod requires nop
        add     sp, sp, #8              @ Update SP.
        ldmfd   sp!, {r0-r3,r12,lr}     @ Restore previous context. (note, lr has spsr)
        msr     spsr_cxsf, r14          @ Restore spsr register from lr.
        @ Stack state: |LR_SVC<-|LR_PREV(PABT)|{original SP_SVC}|
        ldmfd   r13!, {r14, pc}^        @ Return, restoring cpsr. Note r14 gets r14_svc,
                                        @ and pc gets lr_dabt. Saved at #4 and #8 offsets
                                        @ down from where svc stack had left.
END_PROC(arm_prefetch_abort_exception_reentrant)

/*
 * vect_dabt
 *
 * Upon Entry:
 * - R14_abt:   Address of next instruction after aborted instruction
 * - PC:        0x00000010
 * - IRQs are disabled (CPSR[7] = 1)
 *
 *
 * PURPOSE:
 * Used for handling instructions that caused *memory aborts* during
 * the *execution* of the current instruction. This may happen if the
 * instruction accessed a memory address (e.g LDR/STR) that is not
 * defined as part of the currently executing process (aka illegal
 * access). Another possibility is the address is within the address
 * space of the process, but it is not mapped, i.e. does not have
 * physical-to-virtual translation entry in the page tables.
 */
BEGIN_PROC(arm_data_abort_exception)
        sub     lr, lr, #8      @ lr-8 points at aborted instruction
        mrc     p15, 0, r2, c5, c0, 0 @ Read FSR
        mrc     p15, 0, r1, c6, c0, 0 @ Read FAR
        mov     r0, lr          @ Get data abort address
        mov     r5, lr          @ Save it in r5 in case r0 will get trashed
        mov     lr, pc          @ Save return address
        ldr     pc, =data_abort_handler @ Jump to function outside this page.
1:
        b       1b
END_PROC(arm_data_abort_exception)

/*
 * The method of saving abort state to svc stack is identical with that of
 * reentrant irq vector. Natural to this, Restoring of the previous state
 * is also identical.
 */
BEGIN_PROC(arm_data_abort_exception_reentrant)
        clear_exclusive
        sub     lr, lr, #8              @ Get abort address
        str     lr, [r13, #ABT_R14]     @ Store abort address
        mrs     lr, spsr                @ Get SPSR
        str     lr, [r13, #ABT_SPSR]    @ Store SPSR
        str     r0, [r13, #ABT_R0]      @ Store r0
        @ NOTE: Can increase data abort nest here.
        mov     r0, r13                 @ Keep current sp point in R0
        mrs     lr, cpsr                @ Change to SVC mode.
        bic     lr, #ARM_MODE_MASK
        orr     lr, lr, #ARM_MODE_SVC
        msr     cpsr_fc, r14
        @ FIXME: Ensure 8-byte stack here.
        str     lr, [sp, #-8]!  @ Save lr_svc 2 words down from interrupted SP_SVC
transfer_dabt_state_to_svc:
        ldr     lr, [r0, #ABT_R14]
        str     lr, [sp, #4]
        @ Stack state:  |LR_SVC<-|LR_DABT|{original SP_SVC}|
        ldr     lr, [r0, #ABT_SPSR]
        ldr     r0, [r0, #ABT_R0]
        stmfd   sp!, {r0-r3,r12,lr}
        @ Stack state:  |R0<-|R1|R2|R3|R12|DABT_SPSR|LR_SVC|LR_DABT|{original SP_SVC}|
        push_user_sp_lr sp
        @ Stack state:  |SP_USR<-|LR_USR|R0|R1|R2|R3|R12|DABT_SPSR|LR_SVC|LR_DABT|{original SP_SVC}|
read_dabt_state:
        cp15_read_dfsr r1       @ Read DFSR (Tells why the fault occured)
        cp15_read_dfar r2       @ Read DFAR (Contains the faulted data address)
        @ All abort state and (FAR/FSR) saved. Can safely enable irqs here, if need be.
        ldr     r3, [sp, #28]           @ Load DABT_SPSR
        can_abort_enable_irqs r0, r3    @ Judge if irqs can be enabled depending on prev state.
        bne     1f                      @ Branch here based on previous irq judgement.
        enable_irqs r3
1:
        ldr     r0, [sp, #36]           @ Load LR_DABT saved previously.
        mov     lr, pc
        ldr     pc, =data_abort_handler @ Jump to function outside this page.
        disable_irqs r0                 @ Disable irqs to avoid corrupting spsr.
        ldmfd   sp, {sp, lr}^           @ Restore user sp and lr which might have been corrupt on preemption
        nop                             @ User reg mod requires nop
        add     sp, sp, #8              @ Update SP.
        ldmfd   sp!, {r0-r3,r12,lr}     @ Restore previous context. (note, lr has spsr)
        msr     spsr_cxsf, r14          @ Restore spsr register from lr.
        @ Stack state: |LR_SVC<-|LR_PREV(DABT)|{original SP_SVC}|
        ldmfd   r13!, {r14, pc}^        @ Return, restoring cpsr. Note r14 gets r14_svc,
                                        @ and pc gets lr_dabt. Saved at #4 and #8 offsets
                                        @ down from where svc stack had left.
END_PROC(arm_data_abort_exception_reentrant)

/*
 * vect_irq
 *
 * Upon Entry:
 * - R14:       Address of next instruction after interrupted instruction.
 * - PC:        0x00000018
 * - IRQs are disabled (CPSR[7] = 1)
 * - A vectored interrupt controller would also provide where to jump in
 *   order to handle the interrupt, or an irq controller in general would
 *   provide registers that indicate what kind of interrupt has occured.
 *
 *
 * PURPOSE:
 * Used for handling IRQs. IRQs have lower priority compared to other
 * types of exceptions.
 */

/* The most basic handler where neither context switching nor re-entry can occur. */
BEGIN_PROC(arm_irq_exception_basic)
        sub     lr, lr, #4
        stmfd   sp!, {r0-r3,lr}
        mov     lr, pc
        ldr     pc, =do_irq
        ldmfd   sp!, {r0-r3, pc}^
END_PROC(arm_irq_exception)

/* Minimal IRQ state saved on irq stack right after irq vector enters: */
#define IRQ_R0          0
#define IRQ_SPSR        -4
#define IRQ_R14         -8

/* A reentrant handler that uses svc mode stack to prevent banked lr_irq corruption. */
BEGIN_PROC(arm_irq_exception_reentrant)
        sub     lr, lr, #4
@ Save minimal state to irq stack:
        str     r14, [r13, #IRQ_R14]    @ Save lr_irq
        mrs     r14, spsr               @ Copy spsr
        str     r14, [r13, #IRQ_SPSR]   @ Save spsr on irq stack
        str     r0, [r13, #IRQ_R0]      @ Save r0.
        mov     r0, r13                 @ Using r0 to keep banked sp_irq when mode is switched.
        mrs     r14, cpsr               @ Get current psr (irq)
        bic     r14, #ARM_MODE_MASK     @ Clear mode part from psr
        orr     r14, r14, #ARM_MODE_SVC @ Write SVC mode bits.
        msr     cpsr_fc, r14            @ Change to SVC mode.
        str     r14, [r13, #-8]!        @ Save lr_svc 2 words down from where svc stack left.
@ Transfer minimal irq state saved to svc stack:
        ldr     r14, [r0, #IRQ_R14]     @ Load lr_irq to lr using r0 that contains sp_irq.
        str     r14, [r13, #4]          @ Save lr_irq 1 word down from where svc stack left.
        ldr     r14, [r0, #IRQ_SPSR]    @ Load irq spsr.
        ldr     r0, [r0, #IRQ_R0]       @ Restore r0.
        stmfd   sp!, {r0-r3,r12,lr}     @ Save all of rest of irq context to svc stack.
        bl      do_irq                  @ Read irq number etc. Free to re-enable irqs here.
        ldmfd   sp!, {r0-r3-r12,lr}     @ Restore previous context. (note, lr has spsr)
        msr     spsr_cxsf, lr           @ Restore spsr register from lr.
        ldmfd   r13!, {r14, pc}^        @ Return, restoring cpsr. Note r14 gets r14_svc,
                                        @ and pc gets lr_irq. Saved at #4 and #8 offsets
                                        @ down from where svc stack had left.
END_PROC(arm_irq_exception_reentrant)

        .macro  need_resched rx, ry
        get_current \rx
        ldr     \ry, =need_resched_offset
        ldr     \ry, [\ry]
        ldr     \ry, [\rx, \ry]
        cmp     \ry, #1
        .endm

/*
 * Keeps the PSR of the last pre-empted process. This helps to tell
 * what mode the process was in when it was preempted.
 */
.global preempted_psr;
preempted_psr:
.word   0
.word   0
.word   0
.word   0

/* Keeps track of how many nests of irqs have happened. */
.global current_irq_nest_count;
current_irq_nest_count:
.word   0
.word   0
.word   0
.word   0

#if defined (CONFIG_SMP)
        @ Rx contains the address of per cpu variable
        .macro per_cpu adr, temp, varname
                get_cpuid \temp
                ldr \adr, =\varname
                add \adr, \adr, \temp, lsl #2
        .endm
#else
        .macro per_cpu adr, temp, varname
                ldr \adr, =\varname
        .endm
#endif

/*
 * FIXME: current_irq_nest_count also counts for any preempt_disable() calls.
 * However this nesting check assumes all nests come from real irqs.
 * We should make this check just the real ones.
 */
#define IRQ_NESTING_MAX                 32
        .macro  inc_irq_cnt_with_overnest_check rx, ry
        per_cpu \rx, \ry, current_irq_nest_count @ Get per-cpu address of variable
        ldr     \ry, [\rx]
        add     \ry, \ry, #1                    @ No need for atomic inc since irqs are disabled.
        str     \ry, [\rx]
        cmp     \ry, #IRQ_NESTING_MAX           @ Check no more than max nests, and die miserably if so.
        ldrge   pc, =irq_overnest_error
        .endm

        @ This decrement need not be atomic because if you are *decrementing* this, then it means
        @ Preemption is already *disabled*. Ruling out preemption, only race could be against irqs.
        @ If an irq preempts it during decrement and modifies it, it is still responsible to change
        @ it back to the original value as it was when we read it, before it returns. So effectively
        @ anything that runs during the decrement does not affect the value of the count.
        .macro  dec_irq_nest_cnt rx, ry
        per_cpu \ry, \rx, current_irq_nest_count
        ldr     \rx, [\ry]
        sub     \rx, \rx, #1
        str     \rx, [\ry]
        .endm
        .macro in_process_context rx, ry
        per_cpu \rx, \ry, current_irq_nest_count
        ldr     \rx, [\rx]
        cmp     \rx, #0
        .endm
        /* If interrupted a process (as opposed to another irq), saves spsr value to preempted_psr */
        .macro cmp_and_save_process_psr rx, ry
        in_process_context \rx, \ry             @ If nest count is 0, a running process is preempted.
        bne 9999f                               @ Branch ahead if not a process
        per_cpu \rx, \ry, preempted_psr         @ Get per-cpu preempted psr
        mrs     \ry, SPSR                       @ Re-read spsr since register was trashed
        str     \ry, [\rx]                      @ Store it in per-cpu preempted psr
        9999:
        .endm

        /*
         * Clear irq bits on register.
         *
         * If ARMv5, only I-bit is cleared, but if ARMv6-v7,
         * A-bit is also cleared.
         */
        .macro clr_irq_bits_on_reg rx
                bic     \rx, #ARM_IRQ_BIT
#if defined (CONFIG_SUBARCH_V6) || defined (CONFIG_SUBARCH_V7)
                bic     \rx, #ARM_A_BIT
#endif
        .endm

#define CONTEXT_PSR             0
#define CONTEXT_R0              4
#define CONTEXT_R1              8
#define CONTEXT_R2              12
#define CONTEXT_R3              16
#define CONTEXT_R4              20
#define CONTEXT_R5              24
#define CONTEXT_R6              28
#define CONTEXT_R7              32
#define CONTEXT_R8              36
#define CONTEXT_r9              40
#define CONTEXT_R10             44
#define CONTEXT_R11             48
#define CONTEXT_R12             52
#define CONTEXT_R13             56
#define CONTEXT_R14             60
#define CONTEXT_PC              64

/*
 * TODO: Optimization:
 * May use SRS/RFE on irq exception _only_. But not
 * yet aware of its implications. Only irq handler can
 * do it because RFE enables interrupts unconditionally.
 */
BEGIN_PROC(arm_irq_exception_reentrant_with_schedule)
        clear_exclusive
        sub     lr, lr, #4
        str     lr, [r13, #IRQ_R14]     @ Save lr_irq
        mrs     r14, spsr               @ Copy spsr
        str     r14, [r13, #IRQ_SPSR]   @ Save spsr on irq stack
        str     r0, [r13, #IRQ_R0]      @ Save r0.
        cmp_and_save_process_psr r0, r14 @ R14 should have spsr here.
        inc_irq_cnt_with_overnest_check r0, r14
        mov     r0, r13                 @ Using r0 to keep banked sp_irq when mode is switched.
        mrs     r14, cpsr               @ Get current psr (irq)
        bic     r14, #ARM_MODE_MASK     @ Clear mode part from psr
        orr     r14, r14, #ARM_MODE_SVC @ Write SVC mode bits.
        msr     cpsr_fc, r14            @ Change to SVC mode.
        @ FIXME: Ensure 8-byte aligned stack here! Make sure to restore original state later!
        str     r14, [r13, #-8]!        @ Save lr_svc 2 words down from where svc stack left. SP updated.
@ Transfer minimal irq state to svc stack:
        ldr     r14, [r0, #IRQ_R14]     @ Load lr_irq to lr using r0 that contains sp_irq.
        str     r14, [r13, #4]          @ Save lr_irq 1 word down from where svc stack left.
        ldr     r14, [r0, #IRQ_SPSR]    @ Load irq spsr.
        ldr     r0, [r0, #IRQ_R0]       @ Restore r0.
        stmfd   sp!, {r0-r3,r12,lr}     @ Save all of rest of irq context to svc stack.
        mov     lr, pc
        ldr     pc, =do_irq             @ Read irq number etc. Free to re-enable irqs here.
        @ stack state: (Low) r0|r1|r2|r3|r12|SPSR|LR_SVC|LR_IRQ| (High)

/*
 * Decision point for taking the preemption path
 */
#if !defined(CONFIG_PREEMPT_DISABLE)
        per_cpu r0, r1, current_irq_nest_count
        ldr     r0, [r0]
        cmp     r0, #1                  @ Expect 1 as lowest since each irq increase preempt cnt by 1.
        bgt     return_to_prev_context  @ if (irq_nest > 1) return_to_prev_context();
        need_resched r0, r1             @ if (irq_nest == 1 && need_resched) schedule();
        beq     preemption_path         @ if (irq_nest == 1 && !need_resched) return_to_prev_context();
#endif

/*
 * Return to previous context path
 */
return_to_prev_context:
        dec_irq_nest_cnt r0, r1
        disable_irqs r0                 @ Disable irqs to avoid corrupting spsr.
        ldmfd   sp!, {r0-r3,r12,lr}     @ Restore previous context. (note, lr has spsr)
        msr     spsr_cxsf, r14          @ Restore spsr register from lr.
        @ stack state: (Low) |LR_SVC<-|LR_PREV(IRQ)|{original SP_SVC}| (High)
        ldmfd   r13!, {r14, pc}^        @ Return, restoring cpsr. Note r14 gets r14_svc,
                                        @ and pc gets lr_irq. Saved at #4 and #8 offsets
                                        @ down from where svc stack had left.

/*
 * Preemption path
 */
#if !defined(CONFIG_PREEMPT_DISABLE)
preemption_path:
        disable_irqs r0                 @ Interrupts can corrupt stack state.
        get_current r0                  @ Get the interrupted process
        @ stack state: (Low) |->r0|r1|r2|r3|r12|SPSR|LR_SVC|LR_IRQ(<return_address>)| (High)
save_interrupted_context:
        add     sp, sp, #4
        @ stack state: (Low) |r0|->r1|r2|r3|r12|SPSR|LR_SVC|LR_IRQ(<return_address>)| (High)
        ldmfd   sp!, {r1-r3, r12, lr}
        @ stack state: (Low) |r0|..|..|..|..|..|->LR_SVC|LR_IRQ(<return_address>)| (High)
        str     lr, [r0, #CONTEXT_PSR]
        is_psr_usr lr
        add     r0, r0, #CONTEXT_R1     @ Points at register save location for #CONTEXT_R1
        stmia   r0!, {r1-r12}
        ldmfd   sp!, {r1-r2}            @ At this point SP_SVC is at its original svc location.
        @ stack state: (Low) |r0|..|..|..|..|..|..|..|->(Original)| (High)
        @ register state: r0 = (register save loc for #CONTEXT_R13) r1 = LR_SVC, r2 = LR_IRQ
        beq     save_usr_context
save_svc_context:
        stmib   r0, {r1-r2}             @ Save LR_SVC and LR_RETURN in advancing locations.
        str     sp, [r0]                @ Current sp is where sp_svc has left, and r0 at #CONTEXT_SP loc.
        sub     r0, r0, #CONTEXT_R13    @ Go back to first word from SP position.
        ldr     r1, [sp, #-32]          @ Load r0 from stack
        str     r1, [r0, #CONTEXT_R0]   @ Save r0
        b       prepare_schedule        @ All registers saved.
save_usr_context:
        sub     r0, r0, #CONTEXT_R13
        str     r2, [r0, #CONTEXT_PC]   @ Save Program counter
        @ LR_SVC need restoring because it won't be pushed to context frame. SP_SVC is already up-to-date.
        mov     lr, r1
        stmfd   sp, {sp, lr}^   @ Push USR banked regs to stack.
        @ stack state: (Low) |r0|..|..|..|..|..|SP_USR|LR_USR|->(Original)| (High)
        nop                     @ Need a NOP after twiddling with usr registers.
        sub     sp, sp, #8      @ Adjust SP, since stack op on banked regs is no writeback.
        @ stack state: (Low) |r0|..|..|..|..|..|->SP_USR|LR_USR|(Original)| (High)
        ldmfd   sp!, {r1-r2}    @ Pop USR Banked regs.
        @ stack state: (Low) |r0|..|..|..|..|..|..|..|->(Original)| (High)
        str     r1, [r0, #CONTEXT_R13]  @ Save SP_USR to context frame.
        str     r2, [r0, #CONTEXT_R14]  @ Save LR_USR to context frame.
        ldr     r1, [sp, #-32]
        str     r1, [r0, #CONTEXT_R0]
        @ stack state: (Low) |..|..|..|..|..|..|..|..|->(Original)| (High)
prepare_schedule:
        mov lr, pc
        ldr pc, =schedule
1:
        b       1b      /* To catch if schedule returns in irq mode */
#endif /* End of !CONFIG_PREEMPT_DISABLE */

END_PROC(arm_irq_exception_reentrant_with_schedule)

/*
 * Context switch implementation.
 *
 * Upon entry:
 *
 * - r0 = current ktcb ptr, r1 = next ktcb ptr. r2 and r3 = insignificant.
 * - The current mode is always SVC, but the call may be coming from interrupt
 *   or process context.
 * - If coming from interrupt, the interrupted context is already copied to current
 *   ktcb in the irq handler, before coming here. Interrupted context can be SVC or USR.
 *
 * PURPOSE: Handles all paths from irq exception, thread_switch system call,
 * and sleeping in the kernel.
 *
 * NOTES:
 * - If coming from interrupt, the interrupted context is already copied to current
 *   ktcb in the irq handler, before coming here. Interrupted context can be SVC or USR.
 * - If coming from a process context, the current process context need saving here.
 * - From irq contexts, preemption is disabled, i.e. preemption count is 1. This is because
 *   irqs naturally increase preemption count. From process context preemption count is 0.
 *   Process context disables preemption during schedule(), but re-enables before calling
 *   switch_to(). Irq and process contexts are distinguished by preemption_count.
 *   Furthermore, irqs are also disabled shortly before calling switch_to() from both contexts.
 *   This happens at points where stack state would be irrecoverable if an irq occured.
 */
BEGIN_PROC(arch_context_switch)
        clear_exclusive
        in_process_context r2, r3       @ Note this depends on preempt count being 0.
        beq     save_process_context    @ Voluntary switch needs explicit saving of current state.
        dec_irq_nest_cnt r2, r3         @ Soon leaving irq context, so reduce preempt count here.
        b       load_next_context       @ Interrupted context already saved by irq handler.
save_process_context:           @ Voluntary process schedules enter here:
        mrs     r2, cpsr_fc
        str     r2, [r0]
        stmib   r0, {r0-r14}    @ Voluntary scheduling always in SVC mode, so using svc regs.
        str     r14, [r0, #CONTEXT_PC]  @ Store R15 as R14. R14 has return address for switch_to().
load_next_context:
        @ stack state: (Low) |..|..|..|..|..|..|..|..|..|->(Original)| (High)
        mov     sp, r1
        ldr     r0, [sp, #CONTEXT_PSR]  @ Load r0 with SPSR
        clr_irq_bits_on_reg r0          @ Enable irqs on will-be-restored context.
        msr     spsr_fcxs, r0           @ Restore spsr from r0.
        is_psr_usr r0
        bne load_next_context_svc       @ Loading user context is different than svc.
load_next_context_usr:
        ldmib   sp, {r0-r14}^           @ Load all including banked user regs.
        ldr     lr, [sp, #CONTEXT_PC]   @ Load value of PC to r14
        orr     sp, sp, #0xFF0
        orr     sp, sp, #0x8            @ 8-byte aligned.
        movs    pc, lr                  @ Jump to user changing modes.
load_next_context_svc:
        ldmib   sp, {r0-r15}^           @ Switch to svc context and jump, loading R13 and R14 from stack.
                                        @ This is OK since the jump is to current context.
END_PROC(arch_context_switch)


/*
 * vect_fiq
 *
 * Upon Entry:
 * - R14:       Address of next instruction after interrupted instruction.
 * - PC:        0x00000014
 * - FIQs are disabled (CPSR[6] = 1)
 * - IRQs are disabled (CPSR[7] = 1)
 * - As in IRQ, the irq controller would provide registers that indicate
 *   what kind of interrupt has occured.
 *
 * PURPOSE:
 * Handling of high-priority interrupts. FIQs have highest priority after
 * reset and data abort exceptions. They're mainly used for achieving
 * low-latency interrupts, e.g. for DMA.
 */
BEGIN_PROC(arm_fiq_exception)
END_PROC(arm_fiq_exception)

/* * * * * * * * * * * * * * * * * * * * * * * *
 * External functions with absolute addresses  *
 * * * * * * * * * * * * * * * * * * * * * * * */

/*
 * NOTE: Notes on relative and absolute symbols on this file:
 *
 * Note that branches (B and BL) are *RELATIVE* on ARM. So no need to take any
 * special action to access symbols within this file, even though this page
 * (in virtual memory) is relocated to another address at run-time (high or low
 * vectors) - this is an address other than where it is linked at, at
 * compile-time.
 *
 * To access external symbols from this file, (e.g. calling some function in the
 * kernel) one needs to use the: `LDR, pc, =external_symbol' pseudo-instruction,
 * (note the "=") and use absolute addressing. This automatically generates an
 * inline data word within the current module and indirectly loads the value in
 * that word to resolve the undefined reference. All other methods, (LDR, B
 * instructions, or ADR pseudoinstruction) generate relative addresses, and they
 * will complain for external symbols because a relative offset cannot be
 * calculated for an unknown distance. In conclusion, relative branches are
 * useful for accessing symbols on this page, but they mean nothing outside this
 * page, because the page is relocated at run-time. So, wherever you access
 * *relatively* outside this page, would be *relative* to where this page is at
 * that moment.
 */

/* * * * * * * * * * * * * * * * *
 * Stacks for Exception Vectors  *
 * * * * * * * * * * * * * * * * */
.global __stacks_end;
.global __abt_stack_high;
.global __irq_stack_high;
.global __fiq_stack_high;
.global __und_stack_high;

/*
 * These are also linked at high vectors, just as any other symbol
 * on this page.
 */
.balign 4
.equ __abt_stack_high, (__abt_stack - __vector_vaddr + 0xFFFF0000);
.equ __irq_stack_high, (__irq_stack - __vector_vaddr + 0xFFFF0000);
.equ __fiq_stack_high, (__fiq_stack - __vector_vaddr + 0xFFFF0000);
.equ __und_stack_high, (__und_stack - __vector_vaddr + 0xFFFF0000);

/*
 * NOTE: This could be cache line aligned.
 * (use a macro, e.g. ____arm_asm_cache_aligned)
 */
.balign 4

/* 16 bytes each per-cpu, up to 8 cpus */
__stacks_end:   .space 128
__abt_stack:    .space 128
__irq_stack:    .space 128
__fiq_stack:    .space 128
__und_stack:    .space 128