 IF :DEF:|ads$version| 
      PRESERVE8 
     AREA |syscpu$$code|, CODE, READONLY
 ELSE 
    AREA |syscpu$$code|, CODE, READONLY, INTERWORK
 ENDIF

      CODE32

    EXPORT  GSMExit
    EXPORT  DisIntsTimeOverrun
    EXPORT  GSMExitCInitLight
    IMPORT  DumpExitData
    IMPORT  registerBuffer_cpsr
    IMPORT  registerBuffer_usr
    IMPORT  registerBuffer_fiq
    IMPORT  registerBuffer_irq
    IMPORT  registerBuffer_svc
    IMPORT  registerBuffer_abt
    IMPORT  registerBuffer_und
    IMPORT  DoVectorUndefined
    IMPORT  GSMExitIntsDisabledTooLong
    IMPORT  registerBuffer_stack
    IMPORT  pGSMExitHeapStart
    IMPORT  pGSMExitHeapEnd
    IMPORT  pGSMExitStackStart
    IMPORT  pGSMExitStackEnd



























































































































SVC_SWI      EQU 0x00           



GSMExit
    STMFD   sp!, {r0-r3}                    

    MRS     r1, CPSR
    ORR     r0, r1, #0x80:OR:0x40
    MSR     CPSR_c, r0
    LDR     r0, =|registerBuffer_cpsr|      
    STR     r1, [r0]


Flush_DCache_loop
    MRC     p15, 0, pc, c7, c14, 3
    BNE     Flush_DCache_loop
    MOV     r0, #0
    MCR     p15, 0, r0, c7, c10, 4
    MRC     p15,0,r0,c1,c0,0



    AND     r0,r0,#0xFFFFFFF8

    AND     r0,r0,#0xFFFFEFFF
    MCR     p15,0,r0,c1,c0,0


    MSR     cpsr_c, #0x13:OR:(0x80 | 0x40)    
    LDR     r0, =|registerBuffer_svc|
    STMIA   r0!, {r13-r14}                  
    MRS     r1 , spsr
    STMIA   r0!, {r1}                       

    MSR     cpsr_c, #0x11:OR:(0x80 | 0x40)    
    LDR     r0, =|registerBuffer_fiq|
    STMIA   r0!, {r8-r14}                   
    MRS     r1 , spsr
    STMIA   r0!, {r1}                       

    MSR     cpsr_c, #0x12:OR:(0x80 | 0x40)    
    LDR     r0, =|registerBuffer_irq|
    STMIA   r0!, {r13-r14}                  
    MRS     r1 , spsr
    STMIA   r0!, {r1}                       

    MSR     cpsr_c, #0x1B:OR:(0x80 | 0x40)  
    LDR     r0, =|registerBuffer_und|
    STMIA   r0!, {r13-r14}                  
    MRS     r1 , spsr
    STMIA   r0!, {r1}                       

    MSR     cpsr_c, #0x17:OR:(0x80 | 0x40)  
    LDR     r0, =|registerBuffer_abt|
    STMIA   r0!, {r13-r14}                  
    MRS     r1 , spsr
    STMIA   r0!, {r1}                       

    LDR     r0, =|registerBuffer_cpsr|      
    LDR     r0, [r0]
    AND     r0, r0, #0x1F
    ORR     r0, r0, #(0x80 | 0x40)
    MSR     cpsr_c, r0                      

    LDR     r0, =|registerBuffer_usr|
    LDMFD   sp!, {r1}                       
    STMIA   r0!, {r1}                       
    LDMFD   sp!, {r1-r3}                    
    STMIA   r0!, {r1-r12}                   
    MSR     cpsr_c, #0x1F:OR:(0x80 | 0x40)    
    STMIA   r0!, {r13-r15}                  
    MRS     r1 , spsr
    STMIA   r0!, {r1}                       


    MSR     cpsr_c, #0x13:OR:(0x80 | 0x40) 

    LDR     r0, =|pGSMExitHeapStart|
    LDR     r0, [r0]
    LDR     r1, =|pGSMExitHeapEnd|
    LDR     r1, [r1]
    CMP     r13, r0
    BLT     %2
    CMP     r13, r1
    BGT     %2
    LDR     r0, =|registerBuffer_stack|        
    LDR     r1, =(32)*4
    ADD     r1, r0, r1
    MOV     r2, r13
1
    LDR     r3, [r2], #4
    STR     r3, [r0], #4
    CMP     r0, r1
    BLT     %1
2

    LDR     r0, =|registerBuffer_cpsr|      
    LDR     r0, [r0]
    AND     r0, r0, #0x1F
    ORR     r0, r0, #(0x80 | 0x40)
    MSR     cpsr_c, r0                      

   BL      GSMExitCInitLight
    LDR     r0, =|registerBuffer_usr|
    LDR     r0,[r0]
   BL      DumpExitData

WaitLoop
   B       WaitLoop



DisIntsTimeOverrun
    STMFD   sp!, {r0-r3}                    

    LDR     r0, =|registerBuffer_cpsr|      
    MRS     r1, cpsr
    STR     r1, [r0]

    MSR     cpsr_c, #0x13:OR:(0x80 | 0x40)    
    LDR     r0, =|registerBuffer_svc|
    STMIA   r0!, {r13-r14}                  
    MRS     r1 , spsr
    STMIA   r0!, {r1}                       

    LDR     r0, =|pGSMExitHeapStart|
    LDR     r0, [r0]
    LDR     r1, =|pGSMExitHeapEnd|
    LDR     r1, [r1]
    CMP     r13, r0
    BLT     %4
    CMP     r13, r1
    BGT     %4
    LDR     r0, =|registerBuffer_stack|        
    LDR     r1, =(32)*4
    ADD     r1, r0, r1
    MOV     r2, r13
3
    LDR     r3, [r2], #4
    STR     r3, [r0], #4
    CMP     r0, r1
    BLT     %3
4

    MSR     cpsr_c, #0x11:OR:(0x80 | 0x40)    
    LDR     r0, =|registerBuffer_fiq|
    STMIA   r0!, {r8-r14}                   
    MRS     r1 , spsr
    STMIA   r0!, {r1}                       

    MSR     cpsr_c, #0x12:OR:(0x80 | 0x40)    
    LDR     r0, =|registerBuffer_irq|
    STMIA   r0!, {r13-r14}                  
    MRS     r1 , spsr
    STMIA   r0!, {r1}                       

    MSR     cpsr_c, #0x1B:OR:(0x80 | 0x40)  
    LDR     r0, =|registerBuffer_und|
    STMIA   r0!, {r13-r14}                  
    MRS     r1 , spsr
    STMIA   r0!, {r1}                       

    MSR     cpsr_c, #0x17:OR:(0x80 | 0x40)  
    LDR     r0, =|registerBuffer_abt|
    STMIA   r0!, {r13-r14}                  
    MRS     r1 , spsr
    STMIA   r0!, {r1}                       

    LDR     r0, =|registerBuffer_cpsr|      
    LDR     r0, [r0]
    AND     r0, r0, #0x1F
    ORR     r0, r0, #(0x80 | 0x40)
    MRS     r0 , spsr                       

    LDR     r0, =|registerBuffer_usr|
    LDMFD   sp!, {r1}                       
    STMIA   r0!, {r1}                       
    LDMFD   sp!, {r1-r3}                    
    STMIA   r0!, {r1-r12}                   
    MSR     cpsr_c, #0x1F:OR:(0x80 | 0x40)    
    STMIA   r0!, {r13-r15}                  
    MRS     r1 , spsr
    STMIA   r0!, {r1}                       

    LDR     r0, =|registerBuffer_cpsr|      
    LDR     r0, [r0]
    AND     r0, r0, #0x1F
    ORR     r0, r0, #(0x80 | 0x40)
    MRS     r0 , spsr                       



    STMFD   sp!, {r0-r3}                
    MRS     r0, spsr
    STMFD   sp!, {r0,lr}                

    BL      GSMExitCInitLight
    LDR     r1,=GSMExitIntsDisabledTooLong    
    BX      r1


GSMExitCInitLight
   STMFD   sp!, {r0-r2}

    LDR     r0, =|pGSMExitHeapStart|
    LDR     r0, [r0]
    LDR     r1, =|pGSMExitHeapEnd|
    LDR     r1, [r1]
    LDR     r2, =0xbeefcafe
WatermarkHeap_loop
    STR     r2, [r0], #4
    CMP     r0, r1
    BLT     WatermarkHeap_loop

    LDR     r0, =|pGSMExitStackStart|
    LDR     r0, [r0]
    LDR     r1, =|pGSMExitStackEnd|
    LDR     r1, [r1]
    LDR     r2, =0xbeefcafe
WatermarkStack_loop
    STR     r2, [r0], #4
    CMP     r0, r1
    BLT     WatermarkStack_loop

    LDMFD   sp!, {r0-r2}

    LDR     sp, =|pGSMExitStackEnd|
    LDR     sp, [sp]
    BX      lr




      END
