@******************************************************************************
@
@ startup_A15.s - Init code routine for Cortex A15 cores with Linaro GCC
@
@******************************************************************************
@
@ Copyright (C) 2014 Texas Instruments Incorporated - http://www.ti.com/
@
@
@  Redistribution and use in source and binary forms, with or without
@  modification, are permitted provided that the following conditions
@  are met:
@
@    Redistributions of source code must retain the above copyright
@    notice, this list of conditions and the following disclaimer.
@
@    Redistributions in binary form must reproduce the above copyright
@    notice, this list of conditions and the following disclaimer in the
@    documentation and/or other materials provided with the
@    distribution.
@
@    Neither the name of Texas Instruments Incorporated nor the names of
@    its contributors may be used to endorse or promote products derived
@    from this software without specific prior written permission.
@
@  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
@  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
@  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
@  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
@  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
@  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
@  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
@  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
@  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
@  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
@  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@
@******************************************************************************
@Author: Brighton Feng
@Created on June 13, 2014
@****************************** Global Symbols*******************************
        .extern _start 	        @ The symbol _start is the entry point of the runtime support library
        .extern IRQ_Handler
        .extern FIQ_Handler
        .extern Undefined_Handler
        .extern Prefetch_Abort_Handler
        .extern Data_Abort_Handler

        .global Entry
        .global __isr_vector
        .global _FIQ_Handler
        .global _Undefined_Handler
        .global SVC_Handler     @for semihosting, refer to http://processors.wiki.ti.com/index.php/Semihosting
        .global _Prefetch_Abort_Handler
        .global _Data_Abort_Handler
        .global Hypervisor_Handler
        .global _IRQ_Handler

@************************ Internal Definitions ******************************
@ to set the mode bits in CPSR for different modes
        .set  MODE_USR, 0x10            
        .set  MODE_FIQ, 0x11
        .set  MODE_IRQ, 0x12
        .set  MODE_SVC, 0x13
        .set  MODE_MON, 0x16
        .set  MODE_ABT, 0x17
        .set  MODE_HYP, 0x1A
        .set  MODE_UND, 0x1B
        .set  MODE_SYS, 0x1F            

        .equ  I_F_BIT, 0xC0               

@ Define the stack sizes for some modes may be used.
        .set  UND_STACK_SIZE, 0x100
        .set  ABT_STACK_SIZE, 0x100
        .set  FIQ_STACK_SIZE, 0x400
        .set  IRQ_STACK_SIZE, 0x400
        .set  SVC_STACK_SIZE, 0x400
@ HYP mode will not be used in this program.
@ MON mode stack is set by boot ROM code.
@ USR and SYS mode share a stack. 
@ The stack size of the mode when call "_start" will be set with symbol STACKSIZE 
@ in linker option or link script (.lds) file. The space for the stack of the  
@ mode when call "_start" is allocated in the link script (.lds) file. 

@
@ Set the Stack space here
@
        .section special_modes_stack
        .align 4
        .global    SpecialStackEnd
        .global    SpecialStackBase
SpecialStackEnd:
        .space    (UND_STACK_SIZE+ABT_STACK_SIZE+FIQ_STACK_SIZE+IRQ_STACK_SIZE+SVC_STACK_SIZE+8), 0xBF
SpecialStackBase:

@**************************** Code Section ***********************************
        .text
@
@ This code is assembled for ARM instructions
@
        .code 32

@******************************************************************************
@ initalization to be execute in secure mode
@******************************************************************************
    .type    Secure_init, %function
Secure_init:
@===================================================================
@ ACTLR.SMP bit always needs to be set before enabling the caches and MMU,
@ or performing any cache and TLB maintenance operations.
@ In the Cortex-A15 processor, the L1 data cache and L2 cache are always coherent,
@ for shared or non-shared data, regardless of the value of the SMP bit.
@ If ACTLR.SMP is not set the behavior is UNPREDICTABLE.
@===================================================================
        MRC     p15, 0, r0, c1, c0, 1	   @ Read CP15 ACTLR
        ORR     r0, r0, #(1 << 6)		   @ set ACTLR.SMP bit
        MCR     p15, 0, r0, c1, c0, 1	   @ Write CP15 ACTLR

@==================================================================
@ Setup L2 Control Register (L2CTLR) to:
@ 1. workaround errata: ARM L2 Latency is Not Set Correctly by BOOT ROM
@ 2. enable L2 ECC
@==================================================================
        MRC    p15, 1, r0, c9, c0, 2        @read L2CTLR
        BIC    r0, r0, #7                   @clear Data RAM Latency field
        ORR    r0, r0, #3                   @set Data RAM Latency to 3 (4 cycles)
        BIC    r0, r0, #(7<<6)              @clear Tag RAM latency field
        ORR    r0, r0, #(1<<6)              @set Tag RAM Latency to 1 (2 cycles)
        ORR    r0, r0, #(1<<21)             @set ECC and parity enable for L2 and L1
        MCR    p15, 1, r0, c9, c0, 2        @write L2CTLR
        ISB

@==================================================================
@ setup L2 Auxiliary Control Register (L2ACTLR)
@==================================================================
		MRC  p15, 1, r0, c15, c0, 0  @ Read L2 Auxiliary Control Register
        ORR  r0, r0, #(1<<3)      @Disables clean/evict from being pushed to external.
        ORR  r0, r0, #(1<<8)      @Disables DVM and cache maintenance operation message broadcast.
		MCR  p15, 1, r0, c15, c0, 0  @ Write L2 Auxiliary Control Register

@==================================================================
@ clear errors in L2 Extended Control Register from an earlier run
@==================================================================
        MOV    r0, #0     
        MCR    p15, 1, r0, c9, c0, 3        @write L2ECTLR

@
@ Set up the Vector Base Address Regsiter
@
		LDR   r0, =__isr_vector
		MCR   p15, 0, r0, c12, c0, 0 			   @ Write secure VBAR Register

@==================================================================
@ enable NEON/VFP
@==================================================================
		MRC  p15,0,r0,c1,c1,2     @ Read NSACR into Rt
		ORR  r0, r0,#0x00000C00   @ Enable access to NEON/VFP (Coprocessors 10 and 11) in non-secure state
		MCR  p15,0,r0,c1,c1,2     @ Write Rt to NSACR

		MRC  p15,0,r0,c1,c0,2     @ Read CP Access register
		ORR  r0, r0,#0x00f00000   @ Enable full access to NEON/VFP (Coprocessors 10 and 11)
		MCR  p15,0,r0,c1,c0,2     @ Write CP Access register

		ISB

		MOV  r0,#0x40000000       @ Switch on the VFP and NEON hardware
		@VMSR  FPEXC,r0             @ Set EN bit in FPEXC
		.long 0xEEE80A10          @workaround for compiler does not support above instruction

        BX      lr

@
@ Set the Interrupt vector table here
@
    .section .isr_vector
    .align 4
__isr_vector:
        B       Entry
        B       _Undefined_Handler
        B       SVC_Handler
        B       _Prefetch_Abort_Handler
        B       _Data_Abort_Handler
        B       Hypervisor_Handler
        B       _IRQ_Handler
    .type    _FIQ_Handler, %function
_FIQ_Handler:
        SUB     lr, lr, #4                 @ Pre-adjust lr
        SRSDB   sp!, #MODE_FIQ             @ Save lr and SPRS to stack
        PUSH    {r0-r3}                    @ Save APCS corruptible registers to stack
                                           @ (and maintain 8 byte alignment)
        SUB     lr, lr, #4                 @calculate the program address when the interrupt happen
        MOV     r0, lr 		               @pass the LR as 1st parameter (exception happend address)
        LDR     r3, =FIQ_Handler 	       @get the address of FIQ_Handler
        BLX     r3		                   @branch to handler

        POP     {r0-r3}                    @ Restore stacked registers
        RFEIA   sp!                        @ Return from exception

@==================================================================
@ Exception Handlers
@==================================================================
    .type    _Undefined_Handler, %function
_Undefined_Handler:
        @this example simply skip the undefined instruction when return
        @SUB     lr, lr, #4                @ Pre-adjust lr 
        SRSDB   sp!, #MODE_UND             @ Save lr and SPRS to stack
        PUSH    {r0-r3}                    @ Save used registers and APCS corruptible registers to stack
                                           @ (and maintain 8 byte alignment)
        SUB     lr, lr, #4                 @calculate the program address when the exception happen
        MOV     r0, lr 		               @pass the LR as 1st parameter (exception happend address)
        LDR     r3, =Undefined_Handler     @get the address of Undefined_Handler
        BLX     r3		                   @branch to handler

        POP     {r0-r3}                    @ Restore stacked registers
        RFEIA   sp!                        @ Return from exception
 	
    .type    _Prefetch_Abort_Handler, %function
_Prefetch_Abort_Handler:
        @this example return to the instruction after the problematic instruction
        @SUB     lr, lr, #4                @ Pre-adjust lr
        SRSDB   sp!, #MODE_ABT             @ Save lr and SPRS to stack
        PUSH    {r0-r3}                    @ Save used registers and APCS corruptible registers to stack
                                           @ (and maintain 8 byte alignment)
        SUB     lr, lr, #4                 @calculate the program address when the exception happen
        MOV     r0, lr 		               @pass the LR as 1st parameter (exception happend address)
        MOV     r3, #0
        MRC     p15,0,r1,c5,c0,1           @ Read IFSR into R1, pass it as 2nd parameter
        MCR     p15,0,r3,c5,c0,1           @ Write 0 to IFSR
        MRC     p15,0,r2,c6,c0,2           @ Read IFAR into R2, pass it as 3rd parameter
        MCR     p15,0,r3,c6,c0,2           @ Write 0 to IFAR
        LDR     r3, =Prefetch_Abort_Handler @get the address of Prefetch_Abort_Handler
        BLX     r3		                   @branch to handler

        POP     {r0-r3}                    @ Restore stacked registers
        RFEIA   sp!                        @ Return from exception

    .type    _Data_Abort_Handler, %function
_Data_Abort_Handler:
        @this example return to the instruction after the problematic instruction
        SUB     lr, lr, #4                 @ Pre-adjust lr
        SRSDB   sp!, #MODE_ABT             @ Save lr and SPRS to stack
        PUSH    {r0-r3, r11-r12}           @ Save used registers and APCS corruptible registers to stack
                                           @ (and maintain 8 byte alignment)
        SUB     lr, lr, #4                 @calculate the program address when the exception happen
        MOV     r0, lr 		               @pass the LR as 1st parameter (exception happend address)
        MOV     r12, #0
        MRC     p15,0,r1 ,c5,c0,0          @ Read DFSR into R1, pass it as 2nd parameter
        MCR     p15,0,r12,c5,c0,0          @ Write 0 to DFSR
        MRC     p15,0,r2, c6,c0,0          @ Read DFAR into R2, pass it as 3rd parameter
        MCR     p15,0,r12,c6,c0,0          @ Write 0 to DFAR
        MRC     p15,0,r3, c5,c1,0          @ Read Auxiliary Data Fault Status Register to R3, pass it as 4th parameter
        MCR     p15,0,r12,c5,c1,0          @ Write 0 to Auxiliary Data Fault Status Register
        LDR     r12, =Data_Abort_Handler   @get the address of Data_Abort_Handler
        BLX     r12		                   @branch to handler

        POP     {r0-r3, r11-r12}           @ Restore stacked registers
        RFEIA   sp!                        @ Return from exception
        
    .type    _IRQ_Handler, %function
_IRQ_Handler:
        SUB     lr, lr, #4                 @ Pre-adjust lr
        SRSDB   sp!, #MODE_IRQ             @ Save lr and SPRS to stack
        PUSH    {r0-r3}                    @ Save APCS corruptible registers to stack
                                           @ (and maintain 8 byte alignment)
        SUB     lr, lr, #4                 @calculate the program address when the interrupt happen
        MOV     r0, lr 		               @pass the LR as 1st parameter (exception happend address)
        LDR     r3, =IRQ_Handler 	       @get the address of IRQ_Handler
        BLX     r3		                   @branch to handler

        POP     {r0-r3}                    @ Restore stacked registers
        RFEIA   sp!                        @ Return from exception
        
	@for semihosting, refer to http://processors.wiki.ti.com/index.php/Semihosting
    .type    SVC_Handler, %function
SVC_Handler:
        B   	SVC_Handler    			@unused
    
    .type    Hypervisor_Handler, %function
Hypervisor_Handler:
        B   Hypervisor_Handler 	@unused

@******************************************************************************
@ Entry point of program
@ by default, A15 in K2 devices starts to execute application program in supervisor mode
@******************************************************************************
    .type    Entry, %function
Entry:
@******************************************************************************
@ initalize stack pointer for some special modes may be used and disable IRQ/FIQ
@******************************************************************************
@
@ Set up the Stack for Undefined mode
@
         LDR   r0, =SpecialStackBase           @ Read the stack pointer
         SUB   r0, r0, #8 			
         BIC   r0, r0, #7                      @ align to 8-byte boundary
         MSR   cpsr_c, #MODE_UND|I_F_BIT       @ switch to undef  mode
         MOV   sp,r0                           @ write the stack pointer
         SUB   r0, r0, #UND_STACK_SIZE         @ give stack space
@
@ Set up the Stack for abort mode
@
         MSR   cpsr_c, #MODE_ABT|I_F_BIT       @ Change to abort mode
         MOV   sp, r0                          @ write the stack pointer
         SUB   r0,r0, #ABT_STACK_SIZE          @ give stack space
@
@ Set up the Stack for FIQ mode
@
         MSR   cpsr_c, #MODE_FIQ|I_F_BIT       @ change to FIQ mode
         MOV   sp,r0                           @ write the stack pointer
         SUB   r0,r0, #FIQ_STACK_SIZE          @ give stack space
@
@ Set up the Stack for IRQ mode
@
         MSR   cpsr_c, #MODE_IRQ|I_F_BIT       @ change to IRQ mode
         MOV   sp,r0                           @ write the stack pointer
         SUB   r0,r0, #IRQ_STACK_SIZE          @ give stack space
@
@ Set up the Stack for supervisor mode
@
         MSR   cpsr_c, #MODE_SVC|I_F_BIT       @ change to supervisor mode
         MOV   sp,r0                           @ write the stack pointer
@
@ restore to SYS mode
@
         MSR   cpsr_c, #MODE_SYS|I_F_BIT       @ change to SYS mode

@******************************************************************************
@ execute initalization need be done in secure mode by SMC call.
@ In K2 ARM Boot ROM, ISR of SMC will execute the function 
@ pointer stored in R0. User can store functional pointer in R0 and call the 
@ SMC (or SMI) to switch to the Monitor mode and execution the function
@******************************************************************************
		LDR   r0, =Secure_init
		SMC   #0

@==================================================================
@ Disable caches, MMU and branch prediction in case they were left enabled from an earlier run
@ This does not need to be done from a cold reset
@==================================================================
        MRC     p15, 0, r0, c1, c0, 0       @ Read CP15 System Control register
        BIC     r0, r0, #(0x1 << 12)        @ Clear I bit 12 to disable I Cache
        BIC     r0, r0, #(0x1 <<  2)        @ Clear C bit  2 to disable D Cache
        BIC     r0, r0, #0x1                @ Clear M bit  0 to disable MMU
        BIC     r0, r0, #(0x1 << 11)        @ Clear Z bit 11 to disable branch prediction
        MCR     p15, 0, r0, c1, c0, 0       @ Write value back to CP15 System Control register

@==================================================================
@ Invalidate Data and Instruction TLBs and branch predictor in case they were left enabled from an earlier run
@ This does not need to be done from a cold reset
@==================================================================
        MOV     r0, #0
        MCR     p15, 0, r0, c8, c7, 0      @ I-TLB and D-TLB invalidation
        MCR     p15, 0, r0, c7, c5, 6      @ BPIALL - Invalidate entire branch predictor array

@
@ Set up the Vector Base Address Regsiter
@
		LDR   r0, =__isr_vector
		MCR   p15, 0, r0, c12, c0, 0 			   @ Write non-secure VBAR Register

@ jump to _start, the entry point of the runtime support library.
@ The stack for current mode is set by the runtime support library.
@ The BSS section is cleared by the runtime support library.
@ The runtime support library will finally jump to the main function.
         LDR   r10,= _start                    @ Get the address of _start
         MOV   lr,pc                           @ Dummy return 
         BX    r10                             @ Branch to _start function in library 
         SUB   pc, pc, #0x08                   @ looping   

         .end

