#;********************************************************************************
#;*                                                                              *
#;*    Copyright (C) 2002 Oki Electric Industry Co., LTD.                        *
#;*                                                                              *
#;*    System Name    :  ML674000                                                *
#;*    Module Name    :  Startup routine                                         *
#;*    File   Name    :  init.s                                                  *
#;*    Revision       :  01.00                                                   *
#;*    Date           :  2001/12/20  initial version                             *
#;*                                                                              *
#;********************************************************************************

#; the AREA must have the attribute READONLY, otherwise the linker will not
#; place it in ROM.
#;
#; the AREA must have the attribute CODE, otherwise the assembler will not
#; allow any code in this AREA

#        AREA    Init, CODE, READONLY
.text

	.INCLUDE "define.s"
        
#; locations of various things in our memory system
.EQU	RAM_Base,             0x10000000	/* bottom of internal RAM */
.EQU	RAM_Size,             0x2000  		/* size of internal RAM : 8kbyte */
.EQU	RAM_Limit,            RAM_Base+RAM_Size	/* top of internal RAM */

.EQU	IRQ_Stack,            RAM_Limit         /* top of IRQ stack */
.EQU	SVC_Stack,            RAM_Limit-1024    /* top of SVC stack */
.EQU	USR_Stack,            SVC_Stack-1024    /* top of USR stack */

	.extern  IRQ_Handler[WEAK]
	.extern  SWI_irq_en
	.global  SWI_irq_en
	
#; --- Define entry point
   .global  __main
__main:

__entry:
#; --- setup interrupt / exception vectors
        B       Reset_Handler
        B       Undefined_Handler
        B       SWI_Handler
        B       Prefetch_Handler
        B       Abort_Handler
        NOP
        B       IRQ_Handler
        B       FIQ_Handler

#;****************************************
#;*      Undefined Handler               *
#;****************************************
Undefined_Handler:
        B       Undefined_Handler
        
#;********************************
#;*      SWI Handler             *
#;********************************
SWI_Handler:
        STMFD   sp!, {r1-r12,lr}        /* save registers */
        LDR     r1, [lr,#-4]            /* get instruction code */
        BIC     r1, r1, #0xffffff00     /* decode SWI number */
        BIC     r2, r1, #0x0000000f     /* decode SWI_Jump_Table number */
        AND     r1, r1, #0x0000000f
        ADR     r3, SWI_Jump_Table_Table/* get address of jump table table */
        LDR     r4, [r3, r2, LSR #2]    /* get address of jump table */
        LDR     pc, [r4,r1,LSL #2]      /* refer to jump table and branch. */
        
SWI_Jump_Table_Table:
        .long     SWI_Jump_Table
        .long     SWI_PM_Jump_Table
        
SWI_Jump_Table:  						/* normal SWI jump table */
        .long     SWI_irq_en
        .long     SWI_irq_dis
        
SWI_irq_en:      						/* enable IRQ */
        MRS     r0, SPSR        		/* get SPSR */
        BIC     r3, r0, #I_Bit  		/* I_Bit clear */
        AND     r0, r0, #I_Bit  		/* return value */
        MSR     SPSR_c, r3      		/* set SPSR */
        B       EndofSWI
        
SWI_irq_dis:     						/* disable IRQ */
        MRS     r0, SPSR        		/* get SPSR */
        ORR     r3, r0, #I_Bit  		/* I_Bit set */
        AND     r0, r0, #I_Bit  		/* return value */
        MSR     SPSR_c, r3      		/* set SPSR */
        B       EndofSWI
        
SWI_PM_Jump_Table:						/* SWI jump table for power management */
        .long     SWI_pm_if_recov
        .long     SWI_pm_if_dis
        
SWI_pm_if_recov:							/* recover CPSR */
        MSR     SPSR_c, r0      		/* Set SPSR */
        B       EndofSWI
        
SWI_pm_if_dis:							/* mask IRQ and FIQ. and return pre-masked CPSR */
        MRS     r0, SPSR        		/* Get SPSR & Return value */
        ORR     r3, r0, #I_Bit | F_Bit
        MSR     SPSR_c, r3      		/* Set SPSR */
        B       EndofSWI
        
EndofSWI:
        LDMFD   sp!, {r1-r12,pc}^       /* restore registers */
        
#;****************************************
#;*      Prefetch Handler                *
#;****************************************
Prefetch_Handler:
        B       Prefetch_Handler
        
#;********************************
#;*      Abort Handler           *
#;********************************
Abort_Handler:
        B       Abort_Handler
        
#;********************************
#;*      FIQ Handler             *
#;********************************
FIQ_Handler:
        B       FIQ_Handler

#;********************************
#;*      Reset Handler           *
#;********************************
#; the RESET entry point
Reset_Handler:

#; --- initialize stack pointer registers
#; enter IRQ mode and set up the IRQ stack pointer
        MOV     R0, #Mode_IRQ | I_Bit | F_Bit /* no interrupts */
        MSR     CPSR_c, R0
        LDR     R13, =IRQ_Stack 			/* set IRQ mode stack. */

#; set up the SVC stack pointer last and return to SVC mode
        MOV     R0, #Mode_SVC | I_Bit | F_Bit /* no interrupts */
        MSR     CPSR_c, R0
        LDR     R13, =SVC_Stack 			/* set SVC mode STACK. */

#; --- initialize memory required by C code
        
        .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
        beq     	LoopZI    
           
#; --- now change to user mode and set up user mode stack.
        MOV     R0, #Mode_USR | F_Bit | I_Bit
        MSR     CPSR_c, R0
        LDR     sp, =USR_Stack  			/* set USR mode stack. */

#; --- now enable external bus function(second function of PIOA[14:10]).
        LDR     R0, =GPCTL
        LDRH    R1, [R0]
        ORR     R1, R1, #0x4
        STRH    R1, [R0]
        
#; --- now enter the C code
        .extern  main
#    [ :DEF:THUMB
#                ORR     lr, pc, #1
#                BX      lr
#                CODE16                  	/* next instruction will be Thumb */
#    ]                           
        BL      main            			/* branch to main function. */
        
END_LOOP:
        B       END_LOOP