serlasm.s

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

        TTL     Angel serialiser support                 > serlasm.s
        ;
        ; This file provides veneers used by the serialiser module, as
        ; well as some globaly needed functions.
        ;
        ; $Revision: 1.4 $
        ;   $Author: mwelsh $
        ;     $Date: 1999/06/25 15:25:29 $
        ;
        ; Copyright Advanced RISC Machines Limited, 1995.
        ; All Rights Reserved
        ;

        KEEP

        GET     listopts.s           ; standard listing control
        GET     lolevel.s            ; automatically built manifest definitions
        GET     macros.s             ; standard assembler support
        GET     target.s             ; target specific manifests
        GET     taskmacs.s           ; task manipulation macros

        
        ; See serlock.h for the interface to these functions.
        IMPORT  Angel_StackBase
        IMPORT  angel_WaitCore
        IMPORT  Angel_GlobalRegBlock
        IMPORT  angel_SerialiseTaskCore
      IF DEBUG > 0
        IMPORT  angel_DebugTaskArea
        IMPORT  angel_DebugStartTaskCount
        IMPORT  angel_DebugQueueTaskCount
      ENDIF
        
        IMPORT  __rt_asm_fatalerror  ; error reporting via suppasm.s
        
        IMPORT  angel_TQ_Pool
        
        ; ---------------------------------------------------------------------
        ; CONTEXTTABLE
        ; ------------
        ;
        ; CONTEXTTABLE
        ;
        ; 16 byte lookup table used by the exception restore code to
        ; get the offset for the correct r13 for the mode being entered.
        ;
	IF :DEF: ADS_BUILD :LAND: ADS_BUILD = 1
        	AREA    |Serlasm$$RO$$Data|,READONLY
	ELSE
	        AREA    |Serlasm$$RO$$Data|,PIC,READONLY
	ENDIF
        ALIGN   
      IF MINIMAL_ANGEL = 0

        EXPORT  ContextLookuptable
ContextLookuptable

        ; r13 offsets by mode
        DCB     RegOffsR13usr
        DCB     RegOffsR13fiq
        DCB     RegOffsR13irq
        DCB     RegOffsR13svc
        DCB     0, 0, 0
        DCB     RegOffsR13abt
        DCB     0, 0, 0
        DCB     RegOffsR13und
        DCB     0, 0, 0
        DCB     RegOffsR13usr   ; sys mode

        ; r14 offsets by mode
        DCB     RegOffsR14usr
        DCB     RegOffsR14fiq
        DCB     RegOffsR14irq
        DCB     RegOffsR14svc
        DCB     0, 0, 0
        DCB     RegOffsR14abt
        DCB     0, 0, 0
        DCB     RegOffsR14und
        DCB     0, 0, 0
        DCB     RegOffsR14usr   ; sys mode

        ; SPSR offsets by mode
        DCB     RegOffsCPSR     ; no USR SPSR, access CPSR instead
        DCB     RegOffsSPSRfiq
        DCB     RegOffsSPSRirq
        DCB     RegOffsSPSRsvc
        DCB     0, 0, 0
        DCB     RegOffsSPSRabt
        DCB     0, 0, 0
        DCB     RegOffsSPSRund
        DCB     0, 0, 0
        DCB     RegOffsCPSR     ; no SYS SPSR, access CPSR instead
        
      ENDIF

        EXPORT  R13ContextLookuptable
R13ContextLookuptable
        R13LookupTable          ; from taskmacs.s

        ALIGN   

	IF :DEF: ADS_BUILD :LAND: ADS_BUILD = 1
	        AREA    |Serlasm$$RW$$Data|,READWRITE
	ELSE
        	AREA    |Serlasm$$RW$$Data|,PIC,READWRITE
	ENDIF

        ; This is used to store the entry mode for EnterSVC, which
        ; is then used to restore the correct interrupt / mode when
        ; ExitToUSR is called.

angel_SVCEntryMode      % 4
	
	IF :DEF: ADS_BUILD :LAND: ADS_BUILD = 1
	        AREA    |Serlasm$$Code|,CODE,READONLY
	ELSE
        	AREA    |Serlasm$$Code|,CODE,PIC,READONLY
	ENDIF


        ; ****************************************************************
        ;
        ; (*fn)(void) Angel_EnterSVC(void)
        ;
        ; -------------------------------------------
        ;
        ; On Entry:
        ;    <CPSR Mode is either USR or SVC>
        ;
        ; On Exit:
        ;    <CPSR Mode is SVC, IRQ and FIQ flags set>
        ;    r0 <= address of Angel_ExitToUSR()
        ;    r1, r2 trashed (as allowed by APCS)
        ;    lr preserved
        ;    IF entry mode == USR
        ;       sp_svc <= sp_usr
        ;    ENDIF
        ;    angel_SVCEntryFlag <= (CPSR_entry == SVC)
        ;
        ; -------------------------------------------
        ; 
        ; This routine provides a wrapper for the SWI_EnterSVC 
        ; SWI call, allowing entry to Supervisor mode. Note that
        ; unlike the SWI, this call only allows entry from USR
        ; or SVC mode (note: not SYS).
        ; 
        ; The wrapper must note the entry mode to allow routines
        ; which nest this call and so call it in SVC mode to 
        ; return in SVC mode (even though they call ExitToUSR!)
        ; 
        ; These functions need to be revised.
        ; 
        ; The code below assumes that the SWI only trashes those
        ; registers which are allowed by APCS:  r0-3, ip, lr.
        ; 
        ; [Currently, the SWI will only trash r0, r1, lr]
        ; 
        ; -------------------------------------------

        EXPORT  Angel_EnterSVC

InappropriateModeError
        FatalError "Inappropriate Mode\n"

Angel_EnterSVC
        ; a1-a4 and ip may be corrupted under the APCS
        STMFD   sp!, {lr}
        MRS     a1, CPSR
        LDR     a3, =angel_SVCEntryMode
        STR     a1, [a3]
        AND     a2, a1, #ModeMaskUFIS
        CMP     a2, #SVCmode :AND: ModeMaskUFIS
        CMPNE   a2, #FIQmode :AND: ModeMaskUFIS
        CMPNE   a2, #IRQmode :AND: ModeMaskUFIS
        BEQ     WasSVCMode
        CMP     a2, #USRmode :AND: ModeMaskUFIS
        BNE     InappropriateModeError
        ; The SWI does everthing for us - except note that entry was from USR

WasUSRMode
        LDR     a1, =angel_SWIreason_EnterSVC
        SWI     angel_SWI_ARM   ; Get into SVC with IRQ and FIQ disabled
        ; the SWI returns with a1 == Angel_ExitToUSR, so we don't have to 
        ; load it again.
        LDMFD   sp!, {lr}
        MOV     pc, lr          ; Return to caller
        
WasSVCMode      ; Just disable IRQ and FIQ and note SVC entry from SVC
        ORR     a1, a1, #InterruptMask
        MSR     CPSR_cf, a1
        LDR     a1, =Angel_ExitToUSR
        LDMFD   sp!, {lr}       ; note that this is writing to lr_svc,
                                ; not lr_usr, which was what was saved!
        MOV     pc, lr


        ; ****************************************************************
        ;
        ; void Angel_ExitToUSR(void)
        ;
        ; -------------------------------------------
        ;
        ; On Entry:
        ;    <CPSR Mode MUST BE Privileged [unchecked]>
        ;    <angel_SVCEntryMode MUST contain a CPSR as set up
        ;        by Angel_EnterSVC>
        ;
        ; On Exit:
        ;    IF angel_SVCEntryFlag == USR mode value (0)
        ;       <CPSR Mode is USR, IRQ and FIQ flags unset>
        ;       sp_usr = sp_svc
        ;       sp_svc = Top of SVC Stack
        ;    ELSE ; must be SVC mode (1)
        ;       <CPSR Mode is SVC, IRQ and FIQ flags unset>
        ;    ENDIF
        ;    r0, r1, r2 trashed (as allowed by APCS)
        ;
        ; -------------------------------------------
        ; 
        ; This routine nominally returns from SVC mode to USR mode, on
        ; the assumption that the user has just called Angel_EnterSVC. If
        ; however the previous call to Angel_EnterSVC was made in SVC mode,
        ; then we return in SVC mode.
        ; 
        ; In either case, interrupts are enabled on exit, whatever state
        ; they were in on entry.
        ; 
        ; If returning to USR mode, the SVC stack pointer, which was lost on
        ; entry to SVC mode, is returned to the Angel SVC Top of stack value.

        EXPORT  Angel_ExitToUSR

Angel_ExitToUSR
        STMFD   sp!, {lr}
        LDR     a3, =angel_SVCEntryMode
        LDR     a1, [a3]
        AND     a2, a1, #ModeMaskUFIS
        CMP     a2, #SVCmode :AND: ModeMaskUFIS
        CMPNE   a2, #FIQmode :AND: ModeMaskUFIS
        CMPNE   a2, #IRQmode :AND: ModeMaskUFIS
        BEQ     ReturnFromExitToUSR
        CMP     a2, #USRmode :AND: ModeMaskUFIS
        BNE     InappropriateModeError

        ; The standard return to USR case follows ...
        MOV     a3, sp
        STMFD   a3, {a3}        ; copy sp_svc via SVC stack area ...
        SUB     a3, a3, #4
        LDMFD   a3, {sp}^       ; to sp_usr
        
        ; Reset SVC sp to the empty SVC stack
        SetStack Angel_SVCStackOffset
        
ReturnFromExitToUSR
        MSR     CPSR_cxsf, a1        ; return to starting mode...
        LDMFD   sp!, {lr}       ; note that this is writing to lr_svc,
        MOV     pc, lr          ; return to our caller (bank indep.)


     
        ; ****************************************************************
        ;
        ; int Angel_DisableInterruptsFromSVC(void)
        ;
        ; -------------------------------------------
        ; 
        ; Disable (prevent) interrupts. Assumes you are in a supervisor
        ; mode (that is, that MRS and MSR are allowed instructions).
        ;
        ; Passed no parameters;
        ; Returns the previous state of the CPSR
        ;
        ; FIQ SAFETYLEVEL is used to determine which interrupt
        ; flags are set.
        ;
        ; -------------------------------------------

        EXPORT  Angel_DisableInterruptsFromSVC

Angel_DisableInterruptsFromSVC
        MRS     a1, CPSR

        DisableAngelInts a1, a2
        
        MSR     CPSR_cf, a2
        MOV     pc, lr


        ; ****************************************************************
        ;
        ; int Angel_EnableInterruptsFromSVC(void)
        ;
        ; -------------------------------------------
        ; 
        ; Enable (allow) interrupts. Assumes you are in a supervisor
        ; mode (that is, that MRS and MSR are allowed instructions).
        ;
        ; Passed no parameters;
        ; Returns the previous state of the CPSR
        ;
        ; FIQ SAFETYLEVEL and HANDLE_INTERRUPTS_ON_IRQ are used to
        ; determine which interrupt flags are cleared. 
        ;
        ; -------------------------------------------

        EXPORT  Angel_EnableInterruptsFromSVC

Angel_EnableInterruptsFromSVC
        MRS     a1, CPSR

        EnableAngelInts a1, a2
        
        MSR     CPSR_cf, a2
        ;; and return in a1 (r0) the original CPSR...
        MOV     pc, lr


        ; ****************************************************************
        ;
        ; void Angel_RestoreInterruptsFromSVC(int old)
        ;
        ; -------------------------------------------
        ; 
        ; Restore the interrupt flags to the state indicated by 'old'.
        ; Assumes you are in a supervisor mode (that is, that MRS and MSR
        ; are allowed instructions).
        ;
        ; calculates: CPSR = (CPSR & ~IRQMASK) | (a1 & IRQMASK)
        ;
        ; Passed a copy of the CPSR in a1;
        ; Returns nothing.
        ;
        ; FIQ SAFETYLEVEL is used to determine which interrupt
        ; flags are changed.
        ;
        ; -------------------------------------------

        EXPORT Angel_RestoreInterruptsFromSVC

Angel_RestoreInterruptsFromSVC
        ;
        ; Get a copy of CPSR as it is now, and mask out the IRQ bits
        ; and mask from the old CPSR the other (non-IRQ) bits.
        
        MRS     a2, CPSR
        
        AND     a1, a1, #AngelInterruptMask
        BIC     a2, a2, #AngelInterruptMask

        ; now OR the previous value with the new IRQ bits and
        ; put the result back in CPSR. 
        ORR     a1, a2, a1
        MSR     CPSR_cf, a1