/**********************************************************************************************
*File:		startup.s
*Author:	Embest w.h.xie	2005.02.21
*Desc:		lpc22xx\lpc212x\lpc211x\lpc210x startup code
*History:	
*		
*comment:
**********************************************************************************************/

# 7 work mode
.EQU	Mode_USR,             0x10
.EQU	Mode_FIQ,             0x11
.EQU	Mode_IRQ,             0x12
.EQU	Mode_SVC,             0x13
.EQU	Mode_ABT,             0x17
.EQU	Mode_UND,             0x1B
.EQU	Mode_SYS,             0x1F

# interrupt shield bit
.EQU	I_Bit,                0x80
.EQU	F_Bit,                0x40

# state shield bit
.EQU	T_bit,                0x20


# Define the entry point
.globl _start
                .code 32

                .TEXT 
                
_start:

# interrupt Vectors

Vectors:
                LDR     PC, Reset_Addr         
                LDR     PC, Undef_Addr
                LDR     PC, SWI_Addr
                LDR     PC, PAbt_Addr
                LDR     PC, DAbt_Addr
                .long   0xb9205f80          @ keep interrupt vectors sum is 0
                LDR     PC, [PC, #-0xff0]
                LDR     PC, FIQ_Addr

Reset_Addr:
      .long     Reset_Handler
Undef_Addr:
      .long     Undef_Handler
SWI_Addr:
      .long     SWI_Handler
PAbt_Addr:
      .long     PAbt_Handler
DAbt_Addr:
      .long     DAbt_Handler
      .long     0
IRQ_Addr:
      .long     0
FIQ_Addr:
      .long     FIQ_Handler


Undef_Handler:
      B         Undef_Handler
PAbt_Handler:
      B         PAbt_Handler
DAbt_Handler:
      B         DAbt_Handler

SWI_Handler:
                STMFD   sp!, {r0-r3, r12, lr}  
                MOV     r1, sp
                MRS     r0, spsr
                TST     r0, #T_bit
                LDRNEH  r0, [lr,#-2]
                BICNE   r0, r0, #0xFF00
                LDREQ   r0, [lr,#-4]
                BICEQ   r0, r0, #0xFF000000

                # R0 is interrupt number
                # R1 is stack point

                BL      SWI_Exception
                LDMFD   sp!, {r0-r3, r12, pc}^ 


# FIQ_Handler code
FIQ_Handler:    
                STMFD     SP!, {R0-R3, LR}
#                BL           FIQ_Exception
                LDMFD     SP!, {R0-R3, LR}
                SUBS       PC, LR, #4

# The reset handler
Reset_Handler:
                BL       RemapSRAM         @ open MAP
                MRS      R0, CPSR
                AND      R0, R0, #0x1F
                CMP      R0,  #Mode_USR
                SWIEQ    #11
                BL       InitStack
			
#------------------------------------------------------------------------------
#- Initialise C variables
#------------------------
#- Following labels are automatically generated by the linker. 
#- RO: Read-only = the code
#- RW: Read Write = the data pre-initialized and zero-initialized.
#- ZI: Zero-Initialized.
#- Pre-initialization values are located after the code area in the image.
#- Zero-initialized datas are mapped after the pre-initialized.
#- Note on the Data position : 
#- If using the ARMSDT, when no -rw-base option is used for the linker, the 
#- data area is mapped after the code. You can map the data either in internal
#- SRAM ( -rw-base=0x40 or 0x34) or in external SRAM ( -rw-base=0x2000000 ).
#- Note also that to improve the code density, the pre_initialized data must 
#- be limited to a minimum.
#------------------------------------------------------------------------------

                .extern       Image_RO_Limit      	/* End of ROM code (=start of ROM data) */
                .extern       Image_RW_Base       	/* Base of RAM to initialise */           
                .extern       Image_ZI_Base       	/* Base and limit of area */              
                .extern       Image_ZI_Limit      	/* to zero initialise */                  

                ldr         r0, =Image_RO_Limit 	/* Get pointer to ROM data */
                ldr         r1, =Image_RW_Base  	/* and RAM copy */
                ldr         r3, =Image_ZI_Base  	/* Zero init base => top of initialised data */
                cmp         r0, r1                  /* Check that they are different */
                beq         NoRW
LoopRw:         cmp         r1, r3                  /* Copy init data */
                ldrcc       r2, [r0], #4
                strcc       r2, [r1], #4
                bcc         LoopRw
NoRW:           ldr         r1, =Image_ZI_Limit 	/* Top of zero init segment */
                mov         r2, #0
LoopZI:         cmp         r3, r1                  /* Zero init */
                strcc       r2, [r3], #4
                bcc         LoopZI

                .extern  Main
                B        Main

# Setup the stack for each mode
InitStack:
				MOV     R1, LR

				LDR     R0, =Top_Stack

				MSR     CPSR_c, #Mode_UND|I_Bit|F_Bit
				MOV     SP, R0
				SUB     R0, R0, #UND_Stack_Size

				MSR     CPSR_c, #Mode_ABT|I_Bit|F_Bit
				MOV     SP, R0
				SUB     R0, R0, #ABT_Stack_Size

				MSR     CPSR_c, #Mode_FIQ|I_Bit|F_Bit
				MOV     SP, R0
				SUB     R0, R0, #FIQ_Stack_Size

				MSR     CPSR_c, #Mode_IRQ|I_Bit|F_Bit
				MOV     SP, R0
				SUB     R0, R0, #IRQ_Stack_Size

				MSR     CPSR_c, #Mode_SVC|I_Bit|F_Bit
				MOV     SP, R0
				SUB     R0, R0, #SVC_Stack_Size


				MSR     CPSR_c, #Mode_USR
				MOV     SP, R0

				MOV     PC, R1

# Open RemapSRAM
RemapSRAM:
#                STMFD  SP!, {R0-R1,LR}
                
                MOV    R0, #0x40000000
                LDR    R1, =Vectors
                LDMIA  R1!, {R2-R9}
                STMIA  R0!, {R2-R9}
                LDMIA  R1!, {R2-R9}
                STMIA  R0!, {R2-R9}

                LDR    R0, =MEMMAP
                MOV    R1, #0x02
                STR    R1, [R0]
                
                mov    pc,  lr
  
#                LDMFD  SP!, {R0-R1,pc}

.globl   disable_IRQ
.globl   restore_IRQ
.globl   ToSys
.globl   ToUser


# disable IRQ

disable_IRQ:   
               STMFD   SP!, {LR}
               swi     #0
               LDMFD   SP!, {pc}


# restoreIRQ

restore_IRQ:
               STMFD   SP!, {LR}
               swi     #1
               LDMFD   SP!, {pc}

# ToSys work mode

ToSys:
               STMFD   SP!, {LR}
               swi     #11
               LDMFD   SP!, {pc}

# ToUser work mode

ToUser:
               STMFD   SP!, {LR}
               swi     #12
               LDMFD   SP!, {pc}


# Swi interrupt code

SWI_Exception:
               STMFD   SP!, {R2-R3,LR}
               CMP     R0,  #0
               
               MRSEQ   R2,  SPSR
               STREQ   R2,  [R1]
               ORREQ   R2,  R2, #0x80
               MSREQ   SPSR_c, R2
               
               CMP     R0,  #1
               LDREQ   R2,  [R1]
               MSREQ   SPSR_c, R2  
                
               CMP     R0,  #11
               MRSEQ   R2,  SPSR
               BICEQ   R2,  R2, #0x1F
               ORREQ   R2,  R2, #Mode_SYS
               MSREQ   SPSR_c, R2   
                
               CMP     R0,  #12
               MRSEQ   R2,  SPSR
               BICEQ   R2,  R2, #0x1F
               ORREQ   R2,  R2, #Mode_USR
               MSREQ   SPSR_c, R2  
                
               
               LDMFD   SP!, {R2-R3,PC}

  
   .END