taskmacs.s

AT91所有开发板的资料 AT91所有开发板的资料

        SUBT    Macros for serialiser/exception handling                > taskmacs.s
        ; ---------------------------------------------------------------------
        ;
        ; $Revision: 1.1 $
        ;   $Author: rivimey $
        ;     $Date: 1999/03/11 11:53:48 $
        ;
        ; Copyright Advanced RISC Machines Limited, 1995.
        ; All Rights Reserved
        ; ---------------------------------------------------------------------

        ASSERT  (listopts_s)
old_opt SETA    {OPT}
        OPT     (opt_off)   ; disable listing of include files

        ; ---------------------------------------------------------------------

        [       (:LNOT: :DEF: taskmacs_s)

                GBLL    taskmacs_s
taskmacs_s      SETL    {TRUE}


        ; define the register block structure:
                ^ 0

RegOffsR0       # 4             ; offs: 0
RegOffsPC       # 4

RegOffsSPSRfiq  # 4             ; offs: 8
RegOffsR8fiq    # 4
RegOffsR9fiq    # 4
RegOffsR10fiq   # 4
RegOffsR11fiq   # 4
RegOffsR12fiq   # 4
RegOffsR13fiq   # 4
RegOffsR14fiq   # 4

RegOffsSPSRirq  # 4             ; offs: 40 (x28)
RegOffsR13irq   # 4
RegOffsR14irq   # 4

RegOffsSPSRsvc  # 4             ; offs: 52 (x34)
RegOffsR13svc   # 4
RegOffsR14svc   # 4

RegOffsSPSRabt  # 4             ; offs: 64 (x40)
RegOffsR13abt   # 4
RegOffsR14abt   # 4

RegOffsSPSRund  # 4             ; offs: 76 (x4c)
RegOffsR13und   # 4
RegOffsR14und   # 4

RegOffsCPSR     # 4             ; offs: 88 (x58)
RegOffsR1       # 4
RegOffsR2       # 4
RegOffsR3       # 4
RegOffsR4       # 4
RegOffsR5       # 4
RegOffsR6       # 4
RegOffsR7       # 4
RegOffsR8usr    # 4             ; offs: 120 (x78)
RegOffsR9usr    # 4
RegOffsR10usr   # 4
RegOffsR11usr   # 4
RegOffsR12usr   # 4
RegOffsR13usr   # 4
RegOffsR14usr   # 4

RegBlockSize    # 0             ; size: 148 (x94)
        
                ^ 0


        ; Codes for the Angel_DebugLog code
        IF DEBUG <> 0
DL_StartTask    EQU 1
DL_QueueTask    EQU 2
DL_IntHandler   EQU 3
DL_SaveTask     EQU 4
DL_YieldSave    EQU 5
DL_SWISave      EQU 6
DL_UndefSave    EQU 7
DL_WaitSave     EQU 8
DL_AbortSave    EQU 9
DL_DeleteTask   EQU 10
        ENDIF
        
        MACRO
        R13LookupTable

        ; r13 offsets by mode, offset by 2 words (see code in EXCEPTEXIT)
        DCB     RegOffsR13usr - RegOffsSPSRfiq
        DCB     RegOffsR13fiq - RegOffsSPSRfiq
        DCB     RegOffsR13irq - RegOffsSPSRfiq
        DCB     RegOffsR13svc - RegOffsSPSRfiq
        DCB     0, 0, 0
        DCB     RegOffsR13abt - RegOffsSPSRfiq
        DCB     0, 0, 0
        DCB     RegOffsR13und - RegOffsSPSRfiq
        DCB     0, 0, 0
        DCB     RegOffsR13usr - RegOffsSPSRfiq  ; sys mode

        MEND

        ; ---------------------------------------------------------------------
        ; STOREMODE 
        ; -----------
        ;
        ; STOREMODE  modenum
        ;
        ; Using $temp1, $temp2 as scratch, transfer a register frame {spsr, r13, r14}
        ; to the memory at $basereg from the registers for mode $modenum. Increment
        ; r0 over the register block. $cpsrreg has a 'blank' copy of the CPSR, which
        ; should have ints disabled and the mode bits zeroed.
        ;
        MACRO
        STOREMODE $modenum, $basereg, $cpsrreg, $temp1, $temp2

        ORR     $temp1, $cpsrreg, $modenum
        MSR     cpsr_cf, $temp1
        MRS     $temp2, spsr
        STMIA   $basereg!, {$temp2, r13, r14}
        
        MEND


        ; ---------------------------------------------------------------------
        ; EXCEPTENTRY_PART1
        ; -----------
        ; EXCEPTENTRY_PART1 $handler, $regblock
        ;
        ; Code to handle exception handler entry. Parameters include the handler
        ; mode (e.g. IRQmode) and the address to store the registers.
        ;
        ; On Entry:
        ;    sp_<entry mode> should point at an FD stack of a few words.
        ;    CPSR is assumed to have IRQ disabled (as will be the case for
        ;         all exception handlers).
        ;
        ; On Exit:
        ;    r0 will be left pointing into the regblock. r7 contains the original
        ;    value of the CPSR; both are needed by EXCEPTENTRY_PART2.
        ;
        ; After this call, code may use r4-r6 to pass information through to the
        ; code after the EXCEPTENTRY_PART2. Other registers, especially other
        ; modes registers, should be left well alone. Be aware that the whole
        ; sequence is performed interrupts-disabled.
        ;
        
        MACRO
        EXCEPTENTRY_PART1 $handler, $regblock

        STMFD   sp!, {r0}               ; save r0 ...
        MRS     r0, cpsr

      IF $handler <> FIQmode
        ORR     r0, r0, #FIQDisable     ;  disable FIQ's for the moment.
        MSR     cpsr_cf, r0
      ENDIF
        
        STMFD   sp!, {r0, r8}           ; cpsr and r8 on exception handler stack

        ;
        ; save the unbanked regs -- use r8 to address the save area while
        ; we're saving r0 to r7, then move to use r0 for the banked registers
        ;
        LDR     r8, =$regblock + RegOffsCPSR
        MRS     r0, spsr                ; use the SPSR as the saved CPSR value.
        STMIA   r8, {r0-r7}             ; save cpsr along with r1-7 
        SUB     r0, r8, #RegOffsCPSR    ; move base from banked r8 into unbanked r0
                                        ; r0 now addresses RegOffsR0
        LDMFD   sp!, {r7, r8}           ; get orig cpsr, r8 from stack
        LDMFD   sp!, {r1}               ; get orig r0 from stack

        ;
        ; arrange for r14 to point to the 'right' place to return to; it is stored
        ; in the location used to restore PC from, as well as in the 'r14' slot.
        ;
      IF $handler = FIQmode :LOR: $handler = IRQmode
        SUB     r14, r14, #4
      ENDIF
        
      IF  $handler = UNDmode
        SUB     r14, r14, #4    ; This is correct for ARM state only, Thumb State requires
                                ; the LR readjusting for the Thumb increments ( +2)
        
        IF :DEF: THUMB_SUPPORT :LAND: THUMB_SUPPORT<>0
        
          MRS   r2, spsr        ; Test for Thumb state in the saved status register
          TST   r2, #Tbit
          ADDNE r14, r14, #2    ; If Thumb set, add 2 to the lr

        ENDIF                   ; Thumb Support
        
      ENDIF                     ; Undef Handler
        
      IF $handler = ABTmode
        ; The offset of 4 is ok assuming that we accept the abort and continue rather
        ; than trying to fix it, which is OK in Angel as it stands.
        ;
        ; If Angel is changed such that data aborts must be retried, code must be
        ; inserted to distinguish the data abort from a prefetch abort and subtract
        ; #8 rather than #4 before retrying the instruction. 
        SUB     r14, r14, #4
      ENDIF

        MEND
        
        ; ---------------------------------------------------------------------
        ; EXCEPTENTRY_PART2
        ; -----------
        ; EXCEPTENTRY_PART2
        ;
        ; Code to complete exception handler entry. Theis code performs the
        ; handler-independent part of the sequence, saving the various modes'
        ; registers to the regblock. USR mode has already been done. This macro
        ; must be entered in a privileged mode, and EXCEPTENTRY_PART1 must have
        ; been called to set up 'r0' and the other parts of the regblock.
        ;
        ; On Entry:
        ;    r0 must point to the start of the FIQ register block in the regblock
        ;    structure. r0 - r14usr have been saved. r7 contains the original
        ;    value of the CPSR. r4 - r6 may contain useful info and must be
        ;    preserved.
        ;
        ; On Exit:
        ;    r0 will be left pointing at the base of the regblock, which will.
        ;    contain the CPU register state on entry to ENTEREXCEPT_PART1.
        
        MACRO
        EXCEPTENTRY_PART2

        STMIA   r0!, {r1, r14}          ; save r0, lr (as return PC) in memory

        ADD     r2, r0, #RegOffsR8usr - RegOffsSPSRfiq
        STMIA   r2, {r8-r14}^           ; save USR mode registers
        
        ; have now saved unbanked registers. Cycle through the modes and save
        ; the banked ones

        MRS     r1, cpsr                ; make a copy of cpsr with mode bits zero
        BIC     r1, r1, #ModeMask       ; so we can OR in the right value.

        ORR     r2, r1, #FIQmode        ; 
        MSR     cpsr_cf, r2                ; shift to FIQ mode
        MRS     r3, spsr                ; get SPSRfiq
        STMIA   r0!, {r3, r8-r14}       ; save FIQ's banked registers

        ; save the state for the other modes... (input r0, r1, uses r2, r3)
        STOREMODE #IRQmode, r0, r1, r2, r3
        STOREMODE #SVCmode, r0, r1, r2, r3
        STOREMODE #ABTmode, r0, r1, r2, r3
        STOREMODE #UNDmode, r0, r1, r2, r3

        ; restore the mode back to that on entry to the handler
        ; (we kept the cpsr we saved in r7 in the LDMFD)
        MSR     cpsr_cf, r7
        
        ; restore r0 to point at the start of the regblock again.
        SUB     r0, r0, #RegOffsCPSR
        
        MEND

        ; ---------------------------------------------------------------------
        ; EXCEPTENTRY
        ; -----------
        ; EXCEPTENTRY $handler, $regblock
        ;
        ; Code to handle exception handler entry. Parameters include the handler
        ; mode (e.g. IRQmode) and the address to store the registers.
        ;
        ; On Entry:
        ;    sp_<entry mode> should point at an FD stack of a few words.
        ;    CPSR is assumed to have IRQ disabled (as will be the case for
        ;         all exception handlers).
        ;
        ; On Exit:
        ;    r0 will be left pointing at the base of the regblock.
        ;
        
        MACRO
        EXCEPTENTRY $handler, $regblock
        
        EXCEPTENTRY_PART1 $handler, $regblock
        EXCEPTENTRY_PART2
        
        MEND

        ; ---------------------------------------------------------------------
        ; SAVETASK 
        ; -----------
        ; SAVETASK  $regblock, $inregzero
        ;
        ; Code to save the task context. This differs from the EXCEPTENTRY
        ; macro in that it assumes that we start in SVC mode. The task
        ; resumes at the address specified in LR on entry.
        ;
        ; On Entry:
        ;    sp_<entry mode> should point at an FD stack of a few words.
        ;    CPSR is assumed to specify a privileged mode.
        ;    $regblock  is the address to store the registers, if a constant.
        ;    $inregzero is a flag -
        ;        if non-zero, the regblock address is in R0 on entry
        ;        if zero, the regblock address is defined by parameter 1
        ;
        ; On Exit:
        ;    r0 will be left pointing at the base of the regblock.
        ;
        
        MACRO
        SAVETASK $regblock, $inregzero

        STMFD   sp!, {r0}               ; save r0 ...
        MRS     r0, cpsr
        STMFD   sp!, {r0, r8}           ; cpsr and r8 on current mode's stack
        
        ;; ENTER CRITICAL SECTION!
        ORR     r0, r0, #IRQDisable + FIQDisable
        MSR     cpsr_cf, r0        

        ;
        ; save the unbanked regs -- use r8 to address the save area while
        ; we're saving r0 to r7, then move to use r0 for the banked registers
        ;
      IF $inregzero <> 0
        LDR     r8, [sp, #8]            ; load original r0, the ptr to regblock,
        ADD     r8, r8, #RegOffsCPSR    ; from stack and add offset...
      ELSE
        LDR     r8, =$regblock + RegOffsCPSR ; else it's a macro parameter
      ENDIF
        
        MRS     r0, spsr                ; use the SPSR as the saved CPSR value.