serlasm.s

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

        ADD     r0, r0, #RegOffsR4
        LDMIA   r0, {r4-r7}

        ; and return.
        MOV     pc, lr
        
      ENDIF  ; minimal angel

        ; ****************************************************************
        ;
        ; void Angel_Yield(void)
        ;
        ; -------------------------------------------
        ; 
        ; Passed:       nothing
        ; Returns:      nothing
        ; Side-effects: Uses the serialiser. Enables interrupts (temporary).
        ; 
        ; For the interface details of this pseudo function refer to
        ; serlock.h.
        ; 
        ; Note that this an APCS conformant fn from the callers point
        ; of view, so we (only) have to preserve r4-r11, r13 and the
        ; mode we were called in. In this code all registers other than
        ; r0 and r1 are preserved.
        ; 
        ; Angel_Yield is a very complex function, which involves several
        ; state changes. In overview:
        ;   Angel_Yield()       enters SVC mode if not there already
        ;                       saves the current context (to return to)
        ;                       loads the call info for Angel_YieldCore
        ;                       calls Angel_SerialiseTask
        ;   Angel_SerialiseTask creates a context for Angel_YieldCore
        ;                       ... and ASAP enters Angel_YieldCore
        ;                       in SVC mode with interrupts enabled.
        ;   Angel_YieldCore     checks polled devices, if any.
        ;                       returns to the serialiser via NextTask
        ;   angel_NextTask      jumps to SelectNextTask
        ;   Angel_SelectNextTask  resumes the context saved in Angel_Yield,
        ;                       returning to the caller.
        ; 
        ; Because both the SAVETASK macro and the serialiser must be entered
        ; in supervisor mode with (at least the Angel-relevant) interrupts
        ; disabled, and Angel_Yield can be validly called in USR mode, the
        ; routine must check the current mode. If it is USR mode then it
        ; has to call Angel_EnterSVC to enter SVC mode. If in SVC mode we
        ; must ensure that Angel interrupts are disabled first.
        ; 
        ; Having got into the right mode, we must take a copy of the current
        ; context so we can return properly to the caller; a we have changed
        ; this (most notably in changing CPSR) this must be fixed up after
        ; calling SAVETASK.
        ; 
        ; Finally, the parameters for SerialiseTask must be set up, and
        ; at last SerialiseTask can be entered.
        ; 
        ; -------------------------------------------

        
      IF MINIMAL_ANGEL = 0

        EXPORT Angel_Yield
        IMPORT Angel_YieldCore

Angel_Yield
        ; See what mode we are in and get into SVC and disable interrupts
        MRS     r0, CPSR
        ; Save original cpsr in r0 before checking the entry mode
        AND     r1, r0, #ModeMaskUFIS
        CMP     r1, #USRmode :AND: ModeMaskUFIS
        BNE     NotEnteredInUSR
        
        ; In USR mode... must get into SVC mode. 
        STMFD   sp!, {r0, r14}          ; Save the original CPSR and r14
                                        ; in USR stack memory
        BL      Angel_EnterSVC          ; This makes svc_sp := usr_sp

        ; Now:  svc_sp = usr_sp, lr = [pc], r0-r3 (possibly) trashed,
        ; cpsr = {SVCmode, I=1, F=1}, SPSR = USRmode (as on entry).
        ; usr_sp = usr_sp(entry) - 8 (two words pushed earlier)

        ; The two words we pushed before the EnterSVC are popped from svc_sp;
        ; if we didn't do something to usr_sp here we will eventually return
        ; from the yield with the usr stack imbalanced. So, 'pop' these words
        ; from *both* stacks before we continue.
        
        ADD     r0, sp, #8              
        STMFD   sp!, {r0}               ; copy sp_svc via USR stack memory ...
        MOV     r0, sp
        LDMFD   r0, {sp}^               ; to sp_usr
        ADD     r0, r0, #4              ; restore sp to counteract STMFD
        MOV     sp, r0                  ; return to sp_svc
        
        LDMFD   sp!, {r0, r14}          ; now restore r0,14 from svc_sp
                                        ; (still USR memory though)
        
        ; this is required as we never call Angel_ExitToUSR, which would
        ; normally do it for us.
        
        SetStack Angel_SVCStackOffset
        
InSVCInterruptsDisabled
        ; This macro saves the current context to the Yield regblock, setting
        ; the return address to current LR, which *should* be the return PC
        ; for the thing which called Angel_Yield.
        SAVETASK Angel_GlobalRegBlock + (RB_Yield * Angel_RegBlockSize), 0

        LDR     r1, [r0, #RegOffsR0]    ; r0 had the original CPSR
        STR     r1, [r0, #RegOffsCPSR]  ; save it in CPSR ready for return
        
      IF DEBUG <> 0
        ;; r0 is already ok.
        MOV     r1, #0
        MOV     r2, #DL_YieldSave
        BL      Angel_DebugLog
      ENDIF
        
        ;  put regblock address on stack for SerialiseTask
        STMFD   sp!, {r0}
        
        ; Now prepare to call Angel_SerialiseTask
        ; r0 = called_by_yield          ; r1 = fn
        ; r2 = state                    ; r3 = empty_stack
        ; [sp] = interrupted (yield) regblock address (set up above)
        MOV     r0, #1
        LDR     r1, =Angel_YieldCore
        MOV     r2, #0                  ; YieldCore has no parameter
        MOV     r3, #0                  ; empty_stack not used for Yield

        B     Angel_SerialiseTask
        ; and Angel_YieldCore will execute when SerialiseTask allows it to

NotEnteredInUSR
        CMP     r1, #SVCmode :AND: ModeMaskUFIS
        BNE     YieldNotInSvcOrUsrMode
        
YieldInSvcMode
        ; Using the CPSR in r0 (from the start of the routine) create a new
        ; cpsr in r1 with interrupts disabled, then make that current. We
        ; use the original cpsr in r0 -- it must not be trashed.
        
        DisableAngelInts r0, r1
        MSR     cpsr_cf, r1
        B       InSVCInterruptsDisabled
        
YieldNotInSvcOrUsrMode
        ; Not USR or SVC - Oh dear!!!
        FatalError "Yield not USR or SVC mode\n"

      ENDIF  ; minimal angel
        

        ; ****************************************************************
        ;
        ; int Angel_Wait(int signalbits)
        ; r0                      r0
        ;
        ; -------------------------------------------
        ; 
                
      IF MINIMAL_ANGEL = 0

        EXPORT Angel_Wait

Angel_Wait
        ; See what mode we are in and get into SVC and disable interrupts
        MRS     r2, CPSR
        ; Save original cpsr in r2 before checking the entry mode
        AND     r1, r2, #ModeMaskUFIS
        CMP     r1, #USRmode :AND: ModeMaskUFIS
        BNE     WaitNotEnteredInUSR
        
        ; In USR mode... must get into SVC mode. 
        STMFD   sp!, {r0, r2, r14}      ; Save the original r0, CPSR and r14
                                        ; in USR stack memory
        BL      Angel_EnterSVC          ; This makes svc_sp := usr_sp

        ; Now:  svc_sp = usr_sp, lr = [pc], r0-r3 (possibly) trashed,
        ; cpsr = {SVCmode, I=1, F=1}, SPSR = USRmode (as on entry).
        ; usr_sp = usr_sp(entry) - 8 (two words pushed earlier)

        ; The three words we pushed before the EnterSVC are popped from svc_sp;
        ; if we didn't do something to usr_sp here we will eventually return
        ; from the yield with the usr stack imbalanced. So, 'pop' these words
        ; from *both* stacks before we continue.
        
        ADD     r2, sp, #12              
        STMFD   sp!, {r2}               ; copy sp_svc via USR stack memory ...
        MOV     r2, sp
        LDMFD   r2, {sp}^               ; to sp_usr
        ADD     r2, r2, #4              ; restore sp to counteract STMFD
        MOV     sp, r2                  ; return to sp_svc
        
        LDMFD   sp!, {r0, r2, r14}      ; now restore r0,r2,14 from svc_sp
                                        ; (still USR memory though)
        
        ; this is required as we never call Angel_ExitToUSR, which would
        ; normally do it for us.
        
        SetStack Angel_SVCStackOffset
        
InWaitSVCInterruptsDisabled
        ; This macro saves the current context to the Yield regblock, setting
        ; the return address to current LR, which *should* be the return PC
        ; for the thing which called Angel_Yield.
        SAVETASK Angel_GlobalRegBlock + (RB_Yield * Angel_RegBlockSize), 0

        LDR     r1, [r0, #RegOffsR2]    ; r2 had the original CPSR
        STR     r1, [r0, #RegOffsCPSR]  ; save it in CPSR ready for return
        
        ; now have the caller's context saved (apart from r2). Get our
        ; TQI pointer. Leave r0 pointing at the Yield Regblock.
        LDR     r1, =angel_CurrentTask
        LDR     r1, [r1]
        
      IF DEBUG <> 0
        ;; r0, r1 are already ok.
        MOV     r2, #DL_WaitSave
        BL      Angel_DebugLog
      ENDIF

        B       angel_WaitCore

        
WaitNotEnteredInUSR
        CMP     r1, #SVCmode :AND: ModeMaskUFIS
        BNE     WaitNotInSvcOrUsrMode
        
WaitInSvcMode
        ; Using the CPSR in r0 (from the start of the routine) create a new
        ; cpsr in r1 with interrupts disabled, then make that current. We
        ; use the original cpsr in r0 -- it must not be trashed.
        
        DisableAngelInts r2, r1
        MSR     cpsr_cf, r1
        B       InWaitSVCInterruptsDisabled
        
WaitNotInSvcOrUsrMode
        FatalError "Wait not USR or SVC mode\n"

      ENDIF  ; minimal angel
        
        
        ; ****************************************************************
        ;
        ; void angel_NextTask(void)
        ;
        ; -------------------------------------------
        ; 
        ; Passed:       nothing
        ; Returns:      nothing
        ; Side-effects: enables interrupts; calls Angel_Yield, which
        ;               polls the polled interfaces, if any.
        ; 
        ; THIS ROUTINE NEVER RETURNS TO THE CALLER.
        ; 
        ; This 'routine' is used as the veneer between a task which has
        ; completed and the serialiser, which must select a new task to
        ; run. 
        ; 
        ; The routine must ensure the serialiser is called in supervisor
        ; mode with interrupts disables and the Angel supervisor stack in
        ; place; this may require calling Angel_EnterSVC to get into 
        ; supervisor mode from USR mode if the exiting task was a USR mode
        ; callback.
        ; 
        ; It should consequently only ever be entered because a function
        ; executes a MOV pc, lr instruction (or equivalent LDM) with
        ; lr = angel_NextTask; this is set up before the task is run by
        ; Angel_SetupTaskEnvironment.
        ; 
        ; -------------------------------------------

      IF MINIMAL_ANGEL = 0

        IMPORT angel_SelectNextTask
        IMPORT angel_CurrentTask
        IMPORT angel_DeleteTask
        EXPORT angel_NextTask
        
angel_NextTask
        ; This can be "called" from SVC or USR.  It must get into SVC and
        ; disable interrupts and grab the empty SVC stack.
        ; Then it must call angel_SelectNextTask with no arguments.
        ;
        ; See what mode we are in and get into SVC and disable interrupts

        MRS     r0, CPSR
        AND     r1, r0, #ModeMaskUFIS
        CMP     r1, #USRmode :AND: ModeMaskUFIS
        CMPNE   r1, #SYSmode :AND: ModeMaskUFIS
        BNE     NotEnteredInUSR2
        BL      Angel_EnterSVC          ; This keeps the stacks the same
        B       InSVCInterruptsDisabled2

NotEnteredInUSR2
        CMP     r1, #SVCmode :AND: ModeMaskUFIS
        BNE     NextTaskNotUsrOrSvcMode
        
        DisableAngelInts r0, r1
        MSR     cpsr_cf, r1
        
        ; Now in SVC with interrupts disabled
InSVCInterruptsDisabled2
        ; Grab an empty SVC stack and call angel_SelectNextTask
        SetStack Angel_SVCStackOffset

        ; Call angel_DeleteTask with CurrentTask as the task to delete.
        LDR     r0, =angel_CurrentTask
        LDR     r0, [r0]
        BL      angel_DeleteTask

        ; and select another....
        B       angel_SelectNextTask

        
NextTaskNotUsrOrSvcMode 
        FatalError "NextTask not USR or SVC mode\n"
        
      ENDIF  ; minimal angel



        ; ****************************************************************
        ;
        ; Angel_Exchange
	; --------------
        ;
	; Used to support semaphore operations.
	;
        ; -------------------------------------------
        ;
        ; On Entry:
        ;    r0  = address of semaphore in memory
	;    r1  = new value of sem.
        ;
        ; On Exit:
        ;    r0 <= 0 iff semaphore claimed ok
        ;          1 iff mus retry/abandon.
        ;
        ; -------------------------------------------

      IF MINIMAL_ANGEL = 0

	EXPORT  Angel_Exchange
Angel_Exchange
	SWP	r0, r1, [r0]
	MOV	pc, lr

      ENDIF  ; minimal angel

                
        END