GET memory.a
     GET snds.a
     AREA Init, CODE, READONLY
     ; --- Define entry point
     EXPORT __main ; defined to ensure that C runtime system
__main ; is not linked in

     ENTRY

     B Reset_Handler
     B Undefined_Handler
     B SWI_Handler
     B Prefetch_Handler
     B Abort_Handler
     NOP ; Reserved vector
     B IRQ_Handler
     B FIQ_Handler

     ;==========================================================
     ; The Default Exception Handler Vector Entry Pointer Setup
     ;==========================================================
FIQ_Handler
     SUB sp, sp, #4
     STMFD sp!, {r0}
	LDR r0, =HandleFiq
	LDR r0, [r0]
     STR r0, [sp, #4]
     LDMFD sp!, {r0, pc}

IRQ_Handler
     SUB sp, sp, #4
     STMFD sp!, {r0}
	LDR r0, =HandleIrq
     LDR r0, [r0]
     STR r0, [sp, #4]
     LDMFD sp!, {r0, pc}

Prefetch_Handler
     SUB sp, sp, #4
     STMFD sp!, {r0}
	LDR r0, =HandlePrefetch
     LDR r0, [r0]
     STR r0, [sp, #4]
     LDMFD sp!, {r0, pc}

Abort_Handler
     SUB sp, sp, #4
     STMFD sp!, {r0}
	LDR r0, =HandleAbort
     LDR r0, [r0]
     STR r0, [sp, #4]
     LDMFD sp!, {r0, pc}

Undefined_Handler
     SUB sp, sp, #4
     STMFD sp!, {r0}
	LDR r0, =HandleUndef
     LDR r0, [r0]
     STR r0, [sp, #4]
     LDMFD sp!, {r0, pc}

SWI_Handler
     SUB sp, sp, #4
     STMFD sp!, {r0}
	LDR r0, =HandleSwi
     LDR r0, [r0]
     STR r0, [sp, #4]
     LDMFD sp!, {r0, pc}

     AREA Main, CODE, READONLY

     ;=====================================
     ; The Reset Entry Point
     ;=====================================
Reset_Handler

     ;=====================================
     ; Initialise STACK
     ;=====================================

INITIALIZE_STACK
     MRS r0, cpsr
     BIC r0, r0, #LOCKOUT | MODE_MASK
     ORR r2, r0, #USR_MODE
     ORR r1, r0, #LOCKOUT | FIQ_MODE
     MSR cpsr_cf, r1
     MSR spsr_cf, r2

     LDR sp, =FIQ_STACK
     ORR r1, r0, #LOCKOUT | IRQ_MODE
     MSR cpsr_cf, r1
     MSR spsr_cf, r2

     LDR sp, =IRQ_STACK
     ORR r1, r0, #LOCKOUT | ABT_MODE
     MSR cpsr_cf, r1
     MSR spsr_cf, r2

     LDR sp, =ABT_STACK
     ORR r1, r0, #LOCKOUT | UDF_MODE
     MSR cpsr_cf, r1
     MSR spsr_cf, r2

     LDR sp, =UDF_STACK
     ORR r1, r0, #LOCKOUT | SUP_MODE
     MSR cpsr_cf, r1
     MSR spsr_cf, r2
     LDR sp, =SUP_STACK ; Change CPSR to SVC mode

     ;=====================================
     ; LED Display
     ;=====================================
     LDR r1, =IOPMOD
     LDR r0, =0xFF
     STR r0, [r1]

     LDR r1, =IOPDATA
     LDR r0, =0x55
     STR r0, [r1]

	;=====================================
	; UART Setup for Console
	; 38400, data= 8 bits , stop = 1 bits
	;=====================================
	LDR r1, =UARTLCON0
	LDR r0, =0x03
	STR r0,[r1]
	LDR r1, =UARTCONT0
	LDR r0, =0x9
	STR r0,[r1]
	LDR r1, =UARTBRD0

	LDR r0, =0x280 ; 38400 bps for 50MHz

	STR r0,[r1]

	; Print out Boot Up Banner to Console
	LDR r0, =BootUpBanner0
	BL PrintString


EXCEPTION_VECTOR_TABLE_SETUP
     LDR r0, =HandleReset ; Exception Vector Table Memory Loc.
     LDR r1, =ExceptionHandlerTable ; Exception Handler Assign
     MOV r2, #8 ; Number of Exception is 8
     
ExceptLoop
     LDR r3, [r1], #4
     STR r3, [r0], #4
     SUBS r2, r2, #1 ; Down Count
     BNE ExceptLoop

     ;====================================================
     ; Now change to user mode and set up user mode stack.
     ;====================================================
     MRS r0, cpsr
     BIC r0, r0, #LOCKOUT | MODE_MASK
     ORR r1, r0, #USR_MODE
     MSR cpsr_cf, r0
     LDR sp, =USR_STACK

     ; /* Call C_Entry application routine with a pointer to the first */

     ; /* available memory address after ther compiler's global data */
     ; /* This memory may be used by the application. */
     ;===========================
     ; Now we enter the C Program
     ;===========================

     IMPORT C_Entry
     BL C_Entry

     ;===========================================
     ; Exception Vector Function Definition
     ; Consist of function Call from C-Program.
     ;===========================================

SystemUndefinedHandler
     IMPORT ISR_UndefHandler
     STMFD sp!, {r0-r12}
     B ISR_UndefHandler
     LDMFD sp!, {r0-r12, pc}^

SystemSwiHandler
     STMFD sp!, {r0-r12,lr}
     LDR r0, [lr, #-4]
     BIC r0, r0, #0xff000000
     CMP r0, #0xff
     BEQ MakeSVC
     LDMFD sp!, {r0-r12, pc}^

MakeSVC
     MRS r1, spsr
     BIC r1, r1, #MODE_MASK
     ORR r2, r1, #SUP_MODE
     MSR spsr_cf, r2
     LDMFD sp!, {r0-r12, pc}^

SystemPrefetchHandler
     IMPORT ISR_PrefetchHandler
     STMFD sp!, {r0-r12, lr}
     B ISR_PrefetchHandler
     LDMFD sp!, {r0-r12, lr}
     ;ADD sp, sp, #4
     SUBS pc, lr, #4

SystemAbortHandler
     IMPORT ISR_AbortHandler
     STMFD sp!, {r0-r12, lr}
     B ISR_AbortHandler
     LDMFD sp!, {r0-r12, lr}
     ;ADD sp, sp, #4
     SUBS pc, lr, #8

SystemReserv
     SUBS pc, lr, #4

SystemIrqHandler
     IMPORT ISR_IrqHandler
     STMFD sp!, {r0-r12, lr}
     BL ISR_IrqHandler
     LDMFD sp!, {r0-r12, lr}
     SUBS pc, lr, #4

SystemFiqHandler
     IMPORT ISR_FiqHandler
     STMFD sp!, {r0-r7, lr}
     BL ISR_FiqHandler
     LDMFD sp!, {r0-r7, lr}
     SUBS pc, lr, #4

	;======================================================
	; Utility Section
	; Print a zero terminated string
	;======================================================
PrintString
	MOV r4, lr
	MOV r5, r0
01 LDRB r1, [r5],#1 ; read Byte( one character)
	AND r0,r1, #0xff
	TST r0, #0xff
	MOVEQ pc, r4 ; if 0, return.
	BL PutByte
	B %B01
PutByte
PutByteLoop
	LDR r3,=UARTSTAT0
	LDR r2,[r3]
	TST r2,#0x40 ; UART_STAT_XMIT_EMPTY ?
	BEQ PutByteLoop

	LDR r3,=UARTTXH0
	STR r1,[r3] ; write to THR
	MOV pc,lr

     AREA ROMDATA, DATA, READONLY

     ;===========================================
     ; Exception Handler Vector Table Entry Point
     ;===========================================
ExceptionHandlerTable
	 DCD UserCodeArea
     DCD SystemUndefinedHandler
     DCD SystemSwiHandler
     DCD SystemPrefetchHandler
     DCD SystemAbortHandler
     DCD SystemReserv
     DCD SystemIrqHandler
     DCD SystemFiqHandler

     ALIGN
BootUpBanner0 DCB "FS4510 Boot-Up UART TEST !!!",0
DramPostStart DCB &a,&d,"$$$ SNDS100 DRAM Post => ",0
DramPostFinish DCB "MB Installed !",0
CacheEnableBanner DCB &a,&d," $$$ 8K Cache is Enabled !",0

     AREA SYS_STACK, NOINIT

	% USR_STACK_SIZE
USR_STACK

	% UDF_STACK_SIZE
UDF_STACK

	% ABT_STACK_SIZE
ABT_STACK

	% IRQ_STACK_SIZE
IRQ_STACK

	% FIQ_STACK_SIZE
FIQ_STACK

	% SUP_STACK_SIZE
SUP_STACK

 END