taskmacs.s

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

        STMIA   r8, {r0-r7}             ; save cpsr along with r1-7 
        SUB     r0, r8, #RegOffsCPSR    ; move base from banked r8 into r0
                                        ; r0 now addresses RegOffsR0
        LDMFD   sp!, {r7, r8}           ; get orig cpsr, r8 from stack
        LDMFD   sp!, {r1}               ; get orig r0 from stack

        ; in the following code, we use r2 temporarily as a base register,
        ; as r0 will, after saving (r0, pc), be pointing at the right place
        ; for the FIQ save.
        
        STMIA   r0!, {r1, r14}          ; save r0, LR to return PC
        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                ; copy 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 (incl. interrupt flags) back to that on entry
        MSR     cpsr_cf, r7
        ;; LEFT CRITICAL SECTION!

        ; restore r0 to point at the start of the regblock again.
        SUB     r0, r0, #RegOffsCPSR
        
        MEND

        ; ---------------------------------------------------------------------
        ; RESTOREMODE 
        ; -----------
        ;
        ; RESTOREMODE  modenum, $basereg, $cpsrreg, $temp1, $temp2
        ; 
        ; Using two regs as scratch, transfer a register frame {spsr, r13, r14}
        ; from the memory at r0 to the registers for mode $modenum. Increment
        ; $basereg over the register block transferred. $cpsrreg must have a
        ; 'blank' copy of the CPSR.
        ; 
        MACRO
        RESTOREMODE $modenum, $basereg, $cpsrreg, $temp1, $temp2

        ORR     $temp1, $cpsrreg, $modenum
        MSR     cpsr_cf, $temp1
        LDMIA   $basereg!, {$temp2, r13, r14}
        MSR     spsr_cf, $temp2

        MEND


        ; ---------------------------------------------------------------------
        ; EXCEPTEXIT 
        ; -----------
        ; EXCEPTEXIT
        ; 
        ; Code to handle exception handler exit. The macro expects that r0
        ; points to the register context block to restore into the processor,
        ; interrupts are disabled, and the current mode is privileged.
        ;
        ; It assumes that in the context being returned to, there are 2 words
        ; of unused memory at *r13 and *(r13 - 4).

        MACRO
        EXCEPTEXIT
        
        ; firstly, we must modify the SP value to account for the two words
        ; we put on the task's stack when we exit. Which means finding the
        ; right SP first...
        LDR     r1, [r0, #RegOffsCPSR]          ; mode being returned to
        LDR     r4, =R13ContextLookuptable      ; get the address of the r13
                                                ;  lookup table
        AND     r1, r1, #0xF                    ; Mode Mask, 32/26 insensitive
        ; get the SP regblock offset for desired mode
        LDRB    r4, [r4, r1]

        ; now we have SP's offset. Get the two values we want on the task's
        ; stack and read SP into r1. Note that we use the incremented r0, so:
        ; THE R13 LOOKUP TABLE IS OFFSET BY 2 WORDS (see SERLASM)!
        LDMIA   r0!, {r2, r3}
        LDR     r1, [r0, r4]

        ; Now:  r0 -> regblock+8,
        ;       r1 == task's SP,
        ;       r2 == task's R0,
        ;       r3 == task's PC.
        ; Store the R0, PC on the task's stack, incrementing SP, and save this
        ; SP back into the regblock.
        STMFD   r1!, {r2, r3}
        STR     r1, [r0, r4]

        ; get the current CPSR and clear the mode bits so we can ORR in
        ; the desired mode value later.
        MRS     r1, cpsr
        BIC     r1, r1, #ModeMask

        ; Restore first the FIQ state (incl. r8-12), then IRQ, SVC, ABT and UND
        ; states (via the macro) into the regblock
        ORR     r2, r1, #FIQmode
        MSR     cpsr_cf, r2
        LDMIA   r0!, {r3, r8-r14}
        MSR     spsr_cf, r3

        RESTOREMODE #IRQmode, r0, r1, r2, r3
        RESTOREMODE #SVCmode, r0, r1, r2, r3
        RESTOREMODE #ABTmode, r0, r1, r2, r3
        RESTOREMODE #UNDmode, r0, r1, r2, r3

        ; nearly there... restore the USR mode registers (except that r0 isn't
        ; r0, but CPSR) and check for Thumb state
        LDMIA   r0, {r0-r14}^
        [ :DEF: THUMB_SUPPORT :LAND: THUMB_SUPPORT<>0
        TST     r0, #Tbit
        BNE     %F1
        ]
        ; not Thumb, so shift to the right mode and return, via the registers
        ; we stacked earlier.
        ; NOTE: Interrupts may (and can) happen after the MSR!
        
        ; NOP is inserted to ensure no assembler warning for MSR after banked
        ; register operation
                
        NOP     
        MSR     cpsr_cf, r0
        LDMFD   r13!, {r0, pc}

        ; We need to return to Thumb state. Clear the T bit as we're not
        ; allowed to change it with MSR, shift to the desired mode (where,
        ; again, we may end up taking an interrupt) ...
1
        [ :DEF: THUMB_SUPPORT :LAND: THUMB_SUPPORT<>0
        BIC     r0, r0, #Tbit
        MSR     cpsr_cf, r0
        
        ; and BX to a POP which will return to the task.
        ADR     r0, %F2+1
        ;ADD     r0, r0, #1 ; Don't forget we need to set bit 0 for a return
                           ; to Thumb state 
        BX      r0
        CODE16
2
        POP     {r0, pc}

        CODE32
        ]
        MEND


        ; ---------------------------------------------------------------------
        ; RESUMETASK
        ; -----------
        ; RESUMETASK
        ; 
        ; Code to handle task return. The macro expects that r0 points
        ; to the register context block to restore into the processor, that
        ; interrupts are disabled, and that the current mode is privileged.
        ;
        ; It assumes that in the context being returned to, there are 2 words of
        ; unused memory at *r13 and *(r13 - 4).
        ;
        ; It is implemented using EXCEPTEXIT because the code is currently
        ; identical.
        ;

        MACRO
        RESUMETASK

        EXCEPTEXIT

        MEND


        ; ---------------------------------------------------------------------
        ; TAKELOCK
        ; -----------
        ; TAKELOCK $lockvar, $reg1, $reg2
        ; 
        ; Set the lock value addressed by 'lockvar' by adding 1 to it's existing
        ; value. Leave the stored value in $reg2. $reg1 is temporary.
        ;

        MACRO
        TAKELOCK $lockvar, $reg1, $reg2
        LDR     $reg1, =$lockvar 
        LDR     $reg2, [$reg1]
        ADD     $reg2, $reg2, #1
        STR     $reg2, [$reg1]
        MEND
                
        ; ---------------------------------------------------------------------
        ; GIVELOCK
        ; -----------
        ; GIVELOCK $lockvar, $reg1, $reg2
        ; 
        ; Return the lock value addressed by 'lockvar' by subtracting 1 from
        ; it's existing value. Leave the stored value in $reg2. $reg1 is temporary.
        ;

        MACRO
        GIVELOCK $lockvar, $reg1, $reg2
        LDR     $reg1, =$lockvar 
        LDR     $reg2, [$reg1]
        CMP     $reg2, #0
        BNE     %F1
        
        FatalError "Givelock 0."
1       
        SUB     $reg2, $reg2, #1
        STR     $reg2, [$reg1]
        MEND

        ; ---------------------------------------------------------------------
        ; SetStackAndLimit
        ; -----------
        ; SetStackAndLimit $offset, $limit
        ; 
        ; Set up the current mode's SP and SL to the given offsets from the Angel
        ; stack base. $offset should be a value such as Angel_SVCStackOffset and
        ; $limit a value such as Angel_SVCStackLimitOffset.
        ;
        
        MACRO
        SetStackAndLimit $offset, $limit
        [ $offset <> 0
        LDR     sl, =Angel_StackBase
        LDR     sl, [sl]
        ADD     sp, sl, #$offset
        ADD     sl, sl, #$limit
        |       
        LDR     sl, =Angel_StackBase
        LDR     sp, [sl]
        ADD     sl, sp, #$limit
        ]
        MEND
        
        ; ---------------------------------------------------------------------
        ; SetStack
        ; --------
        ; SetStack $offset
        ; 
        ; Set up the current mode's SP to the given offset from the Angel
        ; stack base. $offset should be a value such as Angel_SVCStackOffset
        ;

        MACRO
        SetStack $offset
        LDR     sp, =Angel_StackBase
        LDR     sp, [sp]
        [ $offset <> 0
        ADD     sp, sp, #$offset
        ]
        MEND


       
        ; ---------------------------------------------------------------------
        ; EnsureFIQDisabled
        ; -----------------
        ; EnsureFIQDisabled $psr, $temp
        ; 
        ; This macro is used to disable FIQ in those circumstances, such as
        ; exception handlers, where we already know IRQ is disabled. At the
        ; moment, this only needs to happen when we are servicing interrupts
        ; on FIQ (possibly in addition to IRQ).
        ;

        MACRO
        EnsureFIQDisabled $psr, $temp
        
      IF (HANDLE_INTERRUPTS_ON_FIQ = 1) 
        
        MRS     $temp, $psr
        ORR     $temp, $temp, #FIQDisable
        MSR     $psr._cf, $temp
        
      ENDIF

        MEND    
        
        ; ---------------------------------------------------------------------
        ; DisableAngelInts
        ; ----------------
        ; DisableAngelInts $cpsr_copy, $res_reg
        ; 
        ; On the assumption that $cpsr_copy is an ordinary register with a
        ; copy of the CPSR register for some mode, change it's value by
        ; disabling (setting) those interrupt bits which Angel 'cares' about.
        ;


        MACRO
        DisableAngelInts $cpsr_copy, $res_reg
        
        ORR   $res_reg, $cpsr_copy, #AngelInterruptMask
        
        MEND

        ; ---------------------------------------------------------------------
        ; EnableAngelInts
        ; ---------------
        ; EnableAngelInts $cpsr_copy, $res_reg
        ; 
        ; On the assumption that $cpsr_copy is an ordinary register with a
        ; copy of the CPSR register for some mode, change it's value by
        ; enabling (clearing) those interrupt bits which Angel requires.
        
        MACRO
        EnableAngelInts $cpsr_copy, $res_reg
        
        BIC     $res_reg, $cpsr_copy, #AngelInterruptMask       
        
        MEND
        
        ; ---------------------------------------------------------------------

        ]       ; taskmacs_s

        OPT     (old_opt)   ; restore previous listing options

        ; ---------------------------------------------------------------------
        END     ; EOF taskmacs_s