serlasm.s

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

        MOV     pc, lr

        
        ; ****************************************************************
        ;
        ; int Angel_EnableInterruptsFromUSR(void)
        ;
        ; -------------------------------------------
        ; 
        ; Enable (allow) interrupts. Assumes you are in a USR mode,
        ; so the code must call EnterSVC to go into privileded mode in
        ; order to change the interrupt state.
        ;
        ; Passed no parameters;
        ; Returns the previous state of the CPSR
        ;
        ; -------------------------------------------

        EXPORT  Angel_EnableInterruptsFromUSR

Angel_EnableInterruptsFromUSR
        ; get the current CPSR and check we start in USR mode. Save r0
        ; and LR (which will both be corrupted by the SWI we need to do).
        MRS     r0, CPSR
        STMFD   sp!, {r0, lr}
        
        AND     r0, r0, #ModeMaskUFIS
        CMP     r0, #USRmode :AND: ModeMaskUFIS
        BNE     InappropriateModeError

        ; Get into SVC with IRQ and FIQ disabled and sp_svc <= sp_usr
        LDR     r0, =angel_SWIreason_EnterSVC
        SWI     angel_SWI_ARM
        
        ; now reload LR, CPSR from sp_svc before sp_svc is reset
        LDMFD   sp!, {r0, lr}
        
        ; Reset SVC sp to the empty SVC stack now we've finished with it
        SetStack Angel_SVCStackOffset

        ; do what we really wanted... enable ints again (r0 -> r1)
        EnableAngelInts r0, r1
        MSR     CPSR_cf, r1        ; back in USR mode now.

        ; we pushed two things to sp_usr, and popped from sp_svc, so
        ; we must now readjust sp_usr.
        ADD     sp, sp, #8      
                                ; 
        MOV     pc, lr          ; Return to caller
        
        ; ****************************************************************
        ;
        ; void Angel_SerialiseTask(int fromyield, (*fn)(), int state,
        ;                            int stack, Angel_RegBlock *interrupted)
        ; 
        ; -------------------------------------------
        ; 
        ; Passed:       nothing
        ; Returns:      nothing
        ; Side-effects: Enters the serialiser.
        ; 
        ; For the interface details of this pseudo function refer to
                ; serlock.h. Although it doesn't return (and thus preservation issues
        ; are irrelevant) the parameter passing is APCS conformant.
        ; 
        ; This routine is the entry point into the serialiser from exception
        ; handlers.
        ; 
        ; It must be called from a privileged mode with (Angel-relevant)
        ; interrupts disabled. Typically, it is called from an exception
        ; handler -- the interrupt, SWI, undef handlers all make heavy use
        ; of it. When called from such a handler, the 'stack' value is used
        ; to flatten the caller's mode's stack: this allows the stack at
        ; the point of the call to be unwound ready for the next exception,
        ; a difficult thing to do if the caller is a C program.
        ; 
        ; There is one other big use of this routine: Angel_Yield. See the
        ; comments for that routine for more details. However, the flag
        ; 'fromyield' is set by Angel_Yield to indicate that the stack
        ; parameter is not relevant as Yield managed it's own stacks.
        ; 
        ; The routine's operation is actually relatively simple. The
        ; stacked parameter is read and the stack flattened before the
        ; SAVETASK macro is used to save the current context. This 
        ; context is used as a template to create the new task. It
        ; also holds the other parameters, which must be carefully
        ; switched around to create the desired new context.
        ; 
        ; Bearing in mind that we may have been called in any privileged
        ; mode, we mow modify CPSR to move into SVC mode and ensure the
        ; SVC stack is suitable. Finally, we jump into the coroutine
        ; Angel_SerialiseTaskCore, which performs the task-selection
        ; actions before finally calling Angel_StartTask to execute the
        ; selected task.
        ; 

      IF MINIMAL_ANGEL = 0
        
        EXPORT  Angel_SerialiseTask
        EXPORT  angel_SaveTask

Angel_SerialiseTask
        ; r0 (a1) = called_by_yield
        ; r1 (a2) = fn
        ; r2 (a3) = state
        ; r3 (a4) = base stack pointer for calling ISR
        ; [sp] = interrupted_regblock address
        ;
        ; Note:  r3 is not relevant when called by Angel_Yield.

        ; Load interrupted_regbock for angel_SerialiseTaskCore from the stack,
        ; where it was put by the caller, *before* we flatten the stack.
        LDMFD   sp!, {r5} 

        ; if not called by yield (implication: we have been called by an
        ; interrupt service routine instead), flatten the stack of calling mode
        ; to prepare for the next interrupt).
        CMP     r0, #0
        MOVEQ   sp, r3
	
	MOV	r3, r5
	
        ;        r0 is still 'called by yield' 
        ;        r1 is still 'fn' 
	;        r2 is still 'state'
        ;        r3 is now 'interrupted regblock' arg for S.T.C
        ;
        ; r4 - r9 are dead.

        ; move into SVC mode (previous code could well have been IRQ or FIQ)
        MRS     r4, cpsr
        BIC     r4, r4, #ModeMask
        ORR     r4, r4, #SVCmode
        MSR     cpsr_cf, r4

        ; Note that at this point the SVC stack may be in any of the
        ; following states:
        ;  * Empty Angel_SVCStack - if no task of WantsLock priority
        ;    is running, and the application is not using the SVC stack
        ;  * Non empty Angel_SVCStack - if a task with WantsLock priority
        ;    is running (this  call of serialise task will be due
        ;    to another packet arriving).
        ;  * Application SVC stack
        ;
        ; However, using the Application SVC stack is not acceptable
        ; since it may not be large enough, or indeed r13 may have been
        ; corrupted.  Therefore we must spot sp outside the range
        ; Angel_SVCStack - Angel_SVCStackLimit and if that is the case
        ; the it must be the Application stack in use, so switch to
        ; the Angel_SVCStack (which is empty in this case).
        
        LDR     r4, =Angel_StackBase
        LDR     r4, [r4]
        ADD     sl, r4, #Angel_SVCStackLimitOffset
        ADD     r4, r4, #Angel_SVCStackOffset
        CMP     sp, sl
        BLE     StackNotSetUp      ; sp <= Angel_SVCStackLimit
        CMP     sp, r4
        BMI     StackIsSetUp       ; sp <= Angel_SVCStack
StackNotSetUp
        MOV     sp, r4

StackIsSetUp        
        ; Call angel_SerialiseTaskCore and set up lr to "return" to
        ; angel_NextTask.

        LDR     lr, =angel_NextTask
        LDR     r4, =angel_SerialiseTaskCore
        MOV     pc, r4
        
      ENDIF                   ;  not minimal angel 


        ; ****************************************************************
        ;  void Angel_DebugLog(angel_RegBlock *rb, angel_TaskQueueItem *tq, int id)
        ;
        ; Write to memory (defined as 0x3000000) the following structure
        ;
        ;    [word   taskcount]
        ;    word   id
        ;    word   taskblock (tq)
        ;    word   regblock (rb)
        ;    word   current CPSR
        ;    word   current SP
        ;    word   pc value from regblock (rb)
        ;    word   cpsr value from regblock (rb)
        ;    word   stack pointer from regblock (rb)
        ;    word   r0 from regblock (rb)
        ;
        ; ID codes:      1 == StartTask
        ;                2 == QueueTask
        ;                3 == IntHandler Entry (tq == 0 -> count := starttask)
        ;                4 == SaveTask
        ;                5 == Yield Save
        ;                6 == SWI Save
        ;                7 == Undef (breakpoint) Save
        ;                8 == Wait Save
        ;                9 == Abort Save
        ;               10 == Delete Task
        
      IF MINIMAL_ANGEL = 0 :LAND: DEBUG <> 0

        EXPORT  Angel_DebugLog

Angel_DebugLog
        STMFD   sp!, {r3-r6}
        CMP     r1, #0
        LDRNE   r3, =angel_DebugQueueTaskCount
        LDREQ   r3, =angel_DebugStartTaskCount
        LDR     r6, [r3]
        ADD     r6, r6, #1
        STR     r6, [r3]
        LDR     r3, =angel_DebugTaskArea
        LDR     r4, [r3]
        STR     r6, [r4], #4            ; Store task count
        STR     r2, [r4], #4            ; Store id code
        STR     r0, [r4], #4            ; Store taskblock ptr
        STR     r1, [r4], #4            ; Store regblock ptr
        MRS     r6, CPSR
        STR     r6, [r4], #4            ; Store Current CPSR
        STR     sp, [r4], #4            ; Store Current SP
        LDR     r6, [r0, #RegOffsPC]
        LDR     r5, [r0, #RegOffsCPSR]
        STR     r6, [r4], #4            ; Store the dest pc
        STR     r5, [r4], #4            ; Store the CPSR
        
        ; Now find the stack pointer
        LDR     r6, =ContextLookuptable ; address of r13 lookup table
        AND     r5, r5, #0xF            ; Mode Mask, 32/26 insensitive
        LDRB    r6, [r6, r5]            ; SP offset for desired mode
        LDR     r5, [r0, r6]
        STR     r5, [r4], #4            ; Store the SP

        LDR     r6, [r0, #RegOffsR0]
        STR     r6, [r4], #4            ; Store R0
        
        ; Wrap after 7MB (leave some room for Angel stacks in an 8MB SIMM)
        LDR     r5, =DEBUG_BASE+DEBUG_SIZE
        CMP     r4, r5
        LDRGE   r4, =DEBUG_BASE
        STR     r4, [r3]

        LDMFD   sp!, {r3-r6}
        
        MOV     pc, lr
        
      ENDIF                   ;  not minimal angel 

        ; ****************************************************************
        ;
        ; void angel_StartTask(angel_RegBlock *regblock)
        ;
        ; -------------------------------------------
        ; 
        ; APCS interface to the RESUMETASK macro. Restore the current CPU context,
        ; from the register block pointed to by r0.
        ;
        ; THIS ROUTINE DOES NOT RETURN NORMALLY!
        ;
        ; -------------------------------------------
        

      IF MINIMAL_ANGEL = 0

        EXPORT angel_StartTask

angel_StartTask
        ; r0 (a1) = regblock
        ; see taskmacs.s for details.
      IF DEBUG <> 0
        
        ;; Angel_DebugLog's r0 is already there.
        MOV     r1, #0
        MOV     r2, #DL_StartTask
        BL      Angel_DebugLog

        LDR     r1, =angel_TQ_Pool
        CMP     r0, r1
        BLO     badrberr
        LDR     r1, =Angel_GlobalRegBlock + (RB_NumRegblocks * Angel_RegBlockSize)
        CMP     r0, r1
        BHI     badrberr
        ; 
        LDR     r2, [r0, #RegOffsPC]
        CMP     r2, #0x0006000
        BLO     badpcerr
        ; LDR     r1, =ROMTop	
        ; CMP     r2, r1
        ; BHI     badpcerr
        ; 
        MRS     r2, cpsr
        AND     r2, r2, #ModeMaskUFIS
        CMP     r2, #USRmode :AND: ModeMaskUFIS
        BEQ     badmodeerr

      ENDIF
        
        RESUMETASK 
        ; And the task executes!
        
      IF DEBUG <> 0

badrberr
        MOV     r1, r0
        FatalError "StartTask: Invalid RB: %08lx.\n"

badpcerr
        MOV     r1, r2
        FatalError "StartTask: Invalid PC: %08lx.\n"

badmodeerr
        FatalError "StartTask: Invalid Mode  [cannot be USR mode].\n"

      ENDIF
        
      ENDIF  ; minimal angel

        ; ****************************************************************
        ;
        ; void angel_SaveTask(angel_RegBlock *regblock)
        ;
        ; -------------------------------------------
        ; 
        ; APCS interface to the SAVETASK macro. Saves the current CPU context,
        ; except that r0 is needed for the address and r1 as a temp.
        ;
        ; Callers should not assume a return value (although it will be the
        ; same as the entry parameter).
        ;
        ; -------------------------------------------
        
      IF MINIMAL_ANGEL = 0

angel_SaveTask
        ; r0 (a1) = regblock
        ; see taskmacs.s for details.
        
      IF DEBUG <> 0
        STMFD   sp!, {r0-r2, lr}
        ;; r0 is already ok.
        MOV     r1, #0
        MOV     r2, #DL_SaveTask
        BL      Angel_DebugLog
        
        MRS     r2, cpsr
        AND     r2, r2, #ModeMaskUFIS
        CMP     r2, #USRmode :AND: ModeMaskUFIS
        LDMFD   sp!, {r0-r2, lr}
        BEQ     badmodeerr      ; can't do this in USR mode
      ENDIF
        
        SAVETASK ignored, 1

        ; restore APCS save registers trashed by SAVETASK