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()| (High)

save_interrupted_context:

        add     sp, sp, #4

        @ stack state: (Low) |r0|->r1|r2|r3|r12|SPSR|LR_SVC|LR_IRQ()| (High)

        ldmfd   sp!, {r1-r3, r12, lr}

        @ stack state: (Low) |r0|..|..|..|..|..|->LR_SVC|LR_IRQ()| (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