except.s

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

        LDMFD   sp!, {r0, r1}

        MOV     r0, #-1
        MOV     r1, #-1		; To conform with the semihosting spec.
        MOVS    pc, lr


NotSimpleSWI
        B       ComplexSWI



        LTORG
        
        ; 
        ; This code handles the cases where the SWI causes Scheduler actions
        ; to take place. It encompases three groups of action:
        ;   a. unrecognised SWI numbers
        ;   b. unrecognised Angel SWI reasons
        ;   c. C Library calls such as SWI_READ
        ;   d. Complex Angel actions, such as LateStartup.
        ; 
        ; In order for the scheduler to be callable, we must have saved our
        ; execution context. It is best if this is done with the original
        ; (on entry to handler) context, as this may be used by a debugger
        ; with that meaning.
        ;
        ; We have already disabled FIQ's, so that isn't a problem (remember we
        ; save SPSR as the return CPSR).
        ; 
        ; Sadly, we now have to go check the SWI instruction all over again,
        ; as we can't remember what we've just done :-(
        ; 
SWILockBad
        FatalError "Complex SWI nested: would overwrite context.\n"

CSWIFromAngel
        FatalError "Complex SWI called from within Angel: stack use conflict.\n"

ComplexSWI

        ; Check first that we're not already nested inside another SWI.
        ; We can use r0, r1 here as they are already on the stack.
        LogInfo "except.s", 800, LOG_EXCEPT, "ComplexSWI Takelock\n"
        
        TAKELOCK angel_ComplexSWILock, r0, r1
        CMP     r1, #2
        BHS     SWILockBad      ; bad if lock now 2 or more.

      IF DEBUG = 1
        ; Keep track of the number of calls for debugging etc.
        LDR     r0, =angel_SysCallCount
        LDR     r1, [r0]
        ADD     r1, r1, #1
        STR     r1, [r0]
      ENDIF

        ; restore the CPU state to that at the beginning of the SWI.
        LDMFD   sp!, {r0, r1}
        
        EXCEPTENTRY SVCmode, Angel_GlobalRegBlock + (RB_SWI * Angel_RegBlockSize)
        
        ; The value of r14 is STILL as on entry, i.e. referring to the instruction
        ; after the SWI. r0 points at (the start of) the regblock referred to in
        ; the entry macro.
        
      IF DEBUG = 1
        STMFD   sp!, {r0-r3,lr}
        MOV     r1, #0
        MOV     r2, #DL_SWISave
        BL      Angel_DebugLog
        LDMFD   sp!, {r0-r3,lr}
      ENDIF
        
      IF :DEF: THUMB_SUPPORT :LAND: THUMB_SUPPORT<>0
        
        MRS     r1, SPSR
        TST     r1, #Tbit
        BEQ     CompareWithARMSWI2

        LDRH    r1, [r14, #-2]          ; load the Thumb instruction
        BIC     r1, r1, #0xff00

        LogInfo "except.s", 840, LOG_EXCEPT, "Thumb SWI %lx\n"

        CMP     r1, #angel_SWI_THUMB
        BNE     UnrecognisedSWI
        B       ComplexAngelSWI
        
      ENDIF ; THUMB_SUPPORT

CompareWithARMSWI2
        LDR     r1, [r14, #-4]          ; load the ARM instruction
        BIC     r1, r1, #0xFF000000     ; condition & instr. code, which shouldn't be compared.

        LogInfo "except.s", 852, LOG_EXCEPT, "ARM SWI %lx\n"

        LDR     r2, =angel_SWI_ARM      ; load SWI number (+ SWI instr. code)
        EORS    r2, r2, r1              ; If the values are the same, we'll end up with zero
        BNE     UnrecognisedSWI
        
ComplexAngelSWI
        ; We now know it is our SWI, so we have to interpret the reason code
        ; which was in r0 on entry.
        ;
        ; Here we use the fact that r0 still addresses regblock (see above)
        ;

        LDR     r1, [r0, #RegOffsR0]    ; original r0

      IF MINIMAL_ANGEL = 0

        ; Only recognise the C Library SWIs if Semihosting is enabled
        LDR     r2, =angelOS_SemiHostingEnabled
        LDR     r2, [r2]
        CMP     r2, #0
        BEQ     NonCLibReasonCode
        
        ; Now we know semihosting is switched on
        LDR     r2, =angel_SWIreason_CLibBase
        CMP     r1, r2
        BLO     NonCLibReasonCode     ; orig r0 < angel_SWIreason_CLibBase
        LDR     r2, =angel_SWIreason_CLibLimit
        CMP     r1, r2
        BHI     NonCLibReasonCode     ; orig r0 > angel_SWIreason_CLibLimit

        ; The SWI code needs to run as a normal Angel task -- currently it
        ; will be flagged as TP_Application. Reset the priorities to be
        ; AngelCallBack and set a flag indicating we've done this.
        STMFD   sp!, {r0-r3, lr}
        
        BL      angel_EnterSWI
        
        LDMFD   sp!, {r0-r3, lr}
        
        ; Now we know it is a CLib SWI
        IMPORT  SysLibraryHandler
SysLibraryCall
        LDR     r2, =SysLibraryHandler
        B       DoIndirectCall

      ELSE                          ; minimal angel
        
        B       NonCLibReasonCode
      ENDIF

        ;; Literal pool here if needed.
        LTORG

        
        ; ****************************************************************
        ;
        ; void DoIndirectCall(angel_RegBlock *rb, void *arg, void (*fn)())
        ;
        ; On Entry:     
        ;    r0 contains the address of the caller's regblock
        ;    r1 is used by some functions (e.g. SysLibraryHandler) to pass
        ;       a parameter
        ;    r2 contains the address of the function to be called indirectly
        ;
        ; On Exit:
        ;    <<this code does not return in the normal way>>
        ;    It will return via the state in the SWI RegBlock.
        ; 
        ; ***************************************************************
        ; * This function must be entered in SVC mode from the SWI handler.
        ; ***************************************************************
        ; 
        ; It creates a new USR mode context for the function whose address
        ; is in r2. This function is passed a parameter in register r1
        ; (note this will be the *second* parameter in an APCS "C" function).
        ; 
        ; There is an assumption that a Complex SWI cannot be called as a
        ; result of this function being called. However, simple SWI's (such
        ; as EnterSVC) are allowed. The SWI nesting level is used to verify
        ; this constraint.
        ; 
        ; There is also the more severe assumption that a Complex SWI is
        ; not executed within Angel (by any means). This assumption means
        ; that no Angel stack is in use at the time of the SWI, which in turn
        ; means that the Angel SVC Stack must be available for use (note that
        ; the stack we used earlier *may have been* the application stack,
        ; depending whether the application uses it's own SVC mode stack).
        ; 
        ; With these assumptions, the context we saved on entry to the
        ; Complex SWI code can be used to return to the caller. All we need
        ; to do here is set up the USR mode context:
        ; 
        ;   R0 - R3          -- as for original SWI
        ;   R13usr           -- Caller's SP
        ;   R10usr           -- Caller's SP - <size>
        ;   LR               -- address of SysCallReturnVeneer
        ;   PC               -- value of r2 on entry
        ; 
        ; Note the 'fix' for SL. If the application is not using SL, it
        ; won't have a valid limit set up in SL. Hence, as Angel needs SL
        ; to be valid, we must synthesize a value. The simplest case is to
        ; assume there is space, and set SL = SP - <constant value>, where
        ; the simplest value is SVCStackSize. It might be appropriate to
        ; try to 'second guess' the existing SL to see if it could be a
        ; valid limit already, but that is prone to error too...
        ; 
        ; Finally, this call counts as a kind of 'end of interrupt' point,
        ; so we must flatten the SVC stack before we leave (in the same
        ; way SerialiseTask does for ISR's).
        
      IF MINIMAL_ANGEL = 0
        IMPORT  angel_EnterSWI

DoIndirectCall
        ; Flatten the SVC stack to use the Angel SVC stack.
        SetStackAndLimit Angel_SVCStackOffset, Angel_SVCStackLimitOffset

        LDR     r3, [r0, #RegOffsCPSR]  ; get the mode being returned to
        AND     r3, r3, #0xF            ; Mode Mask, 32/26 insensitive
        LDR     r4, =ContextLookuptable ; get the address of the r13 lookup
                                        ;  table (r13 is first)
        LDRB    r4, [r4, r3]            ; get SP offset for desired mode
        LDR     r3, [r0, r4]            ; get actual SP for mode

        ; this use of r10/sl is FIQ safe -- we're in SVC, going to USR, so
        ; r10 is always R10usr. See comments above for the purpose of this.
        SUB     sl, r3, #Angel_SVCStackSize
                                        ; get calculated SLusr from SP.
        
        ; This may be appropriate too:
        MOV     fp, #0                  ; start of call-frame
        
        LDR     r4, =SysCallReturnVeneer
        
        ; would normally be sp! in both, but we're not supposed to use
        ; this form of LDM with '!' so don't do either with '!'.
        STMFD   sp, {r3, r4}            ; save the new User SP, LR on Angel SVC stack
        SUB     r3, sp, #8
        LDMFD   r3, {r13, r14}^         ; Load back into R13usr, R14usr
        
        MRS     r3, cpsr
        BIC     r3, r3, #ModeMask
        ; EnableAngelInts r3, r3          ; or else comms things won't work...
        ORR     r3, r3, #USRmode
        MSR     spsr_cf, r3

        LogInfo "except.s", 999, LOG_EXCEPT, "DoIndirectCall: jumping to routine\n"

        MOV     lr, r2                  ; get fn to call in safe place.

        ; r0 is still pointing at the regblock. Read in r1 - r3 from the
        ; regblock, then read r0 
        ADD     r0, r0, #RegOffsR1
        LDMIA   r0, { r1, r2, r3 }
        LDR     r0, [ r0, #RegOffsR0 - RegOffsR1 ] ; neg offset back to R0
        
        ; And leap into the indirectly called routine in USR mode
        MOVS    pc, lr

      ENDIF ; Minimal Angel

        
        LTORG
       
        ; ****************************************************************
        ;
        ; void SysCallReturnVeneer(unsigned retval)
        ;
        ; On Entry:     
        ;    r0 contains the return value from the syscall
        ;    r1-r9, r11, r12 are undefined.
        ;    r10 should be the SL value as written in DoIndirectCall
        ;    r13 should be the SP value as written in DoIndirectCall
        ;    r14 will be the address of this function.
        ;
        ; On Exit:
        ;    <<this code does not return in the normal way>>
        ;    It will return via the state in the SWI RegBlock.
        ; 
        ; ***************************************************************
        ; * This function expects to be entered in USR mode.
        ; ***************************************************************
        ;
        ; 
      IF MINIMAL_ANGEL = 0

        IMPORT  angel_ExitSWI

SysCallReturnVeneer   
        ; Save the return value in R0 from the SWI into the regblock
        LDR     r7, =Angel_GlobalRegBlock + (RB_SWI * Angel_RegBlockSize)
        STR     r0, [r7, #RegOffsR0]

        ; Need to be in a privileged mode to do the restore; use EnterSVC to
        ; do this.  (This is ok, as EnterSVC is a Simple SWI, not a Complex
        ; one, and thus doesn't use the SWI regblock).
        LDR     r0, =angel_SWIreason_EnterSVC
        SWI     angel_SWI_ARM

        LogInfo "except.s", 1052, LOG_EXCEPT, "SysCallReturnVeneer Givelock\n"
        
        GIVELOCK angel_ComplexSWILock, r0, r1

        ; restore task priorities and the 'in swi' flag.
        STMFD   sp!, {r0-r3, lr}
        BL      angel_ExitSWI
        LDMFD   sp!, {r0-r3, lr}

        LDR     r0, =angel_SWIDeferredBlock
        LDR     r1, [r0]
        CMP     r1, #0
        BNE     SysCallDeferredBlock
        
        ; Now in SVC mode. r7 should still point to the register block,
        ; so get it into r0 and branch to StartTask to return to the
        ; called function
        
        LDR     r0, =Angel_GlobalRegBlock + (RB_SWI * Angel_RegBlockSize)
        B       angel_StartTask
        
SysCallDeferredBlock

        ; first off, zero deferred block flag ready for next time
        MOV     r1, #0
        STR     r1, [r0]
	B	Angel_DeferredBlockCore
	
      ENDIF
        
        ; **********************************************************

        ; Deal with cases other than the semi-hosted CLib SWI
        ; r0 = our regblock with the saved regs stored
        ; r1 = the original r0 when we get here
NonCLibReasonCode

      IF MINIMAL_ANGEL = 0
        IMPORT  angel_ApplDeviceHandler

        ;; ApplDevice only known on full Angel, non-EmbeddedICE systems
        CMP     r1, #angel_SWIreason_ApplDevice
        BNE     %F1
        
        LDR     r2, =angel_ApplDeviceHandler
        B       DoIndirectCall
        
1
      ENDIF
        
        LDR     r2, =angel_SWIreason_ReportException
        CMP     r1, r2
        BNE     UnrecognisedSWI
        
      IF MINIMAL_ANGEL <> 0
        ; We cannot report an exception within ICEMan
ReportException
        B       ReportException
      ELSE
        ; this is a bit early (we haven't quite finished with the regblock, as
        ; SerialiseTask wants it), but ok as we won't enable ints until it
        ; really is ok.
        LogInfo "except.s", 1114, LOG_EXCEPT, "ReportException Givelock\n"
        
        GIVELOCK angel_ComplexSWILock, r0, r2 
        
        ; The ADP_Stopped code was in r1 before the SWI happened -- load it
        LDR     r0, =Angel_GlobalRegBlock + (RB_SWI * Angel_RegBlockSize)
        LDR     r2, [r0, #RegOffsR1]
        BL      CallSerialiseTask
      ENDIF

UnrecognisedSWI
      IF MINIMAL_ANGEL<>0
        ; We cannot report an unrecognised SWI within ICEMan
        B       UnrecognisedSWI
        
      ELSE
        ; this is a bit early (we haven't quite finished with the regblock, as
        ; SerialiseTask wants it), but ok as we won't enable ints until it
        ; really is ok.
        LogInfo "except.s", 1133, LOG_EXCEPT, "UnrecognisedSWI Givelock\n"
        
        GIVELOCK angel_ComplexSWILock, r0, r2
        
        LDR     r0, =Angel_GlobalRegBlock + (RB_SWI * Angel_RegBlockSize)
        LDR     r2, =ADP_Stopped_SoftwareInterrupt
        BL      CallSerialiseTask

      ENDIF

        END