;==================================================================
; Copyright ARM Ltd 2005-2012. All rights reserved.
;
; Cortex-A7 Embedded example - Startup Code
;
; This is a merged version of crt0_armcc.s and original ARM
; DS-5 startup file
;==================================================================


; Standard definitions of mode bits and interrupt (I & F) flags in PSRs

Mode_USR        EQU     0x10
Mode_FIQ        EQU     0x11
Mode_IRQ        EQU     0x12
Mode_SVC        EQU     0x13
Mode_ABT        EQU     0x17
Mode_UND        EQU     0x1B
Mode_SYS        EQU     0x1F
Mode_Mask 		EQU 	0x1F

I_Bit           EQU     0x80               ; When I bit is set, IRQ is disabled
F_Bit           EQU     0x40               ; When F bit is set, FIQ is disabled


    PRESERVE8
    ;AREA   VECTORS, CODE, READONLY         ; Name this block of code
    AREA   Crt0, CODE, READONLY      ; name this block of code	; Use to match original sample code.  This section is in vectors_armcc.s (entry point) then that calls crt0_armcc.s

    ENTRY

	
    ; added from original sample code
	IMPORT  ||Image$$ARM_LIB_STACK$$ZI$$Limit||
	IMPORT  ||Image$$IRQ_STACK$$ZI$$Limit||
	IMPORT  ||Image$$FIQ_STACK$$ZI$$Limit||

;==================================================================
; Entry point for the Reset handler
;==================================================================

    EXPORT Start
    EXPORT _start	; to match original sample code symbol

Start
_start
;======================================================================
; BEGIN: imported initialization from crt0_armcc.s - orig sample code
;======================================================================
	ldr    r1,=0x0
	mov    r2,  r1
	mov    r3,  r1
	mov    r4,  r1
	mov    r5,  r1
	mov    r6,  r1
	mov    r7,  r1
	mov    r8,  r1
	mov    r9,  r1
	mov    r10, r1
	mov    r11, r1
	mov    r12, r1

    MRS     r0, cpsr                ; Original PSR value

    ;; Set up the interrupt stack pointer.

    BIC     r0, r0, #Mode_Mask       ; Clear the mode bits
    ORR     r0, r0, #Mode_IRQ       ; Set IRQ mode bits
    MSR     cpsr_c, r0              ; Change the mode
    ;LDR     sp, =__initial_irqsp     ; End of IRQ_STACK
    LDR     sp, =||Image$$IRQ_STACK$$ZI$$Limit||     ; End of IRQ_STACK
    BIC     sp,sp,#0x7              ; Make sure SP is 8 aligned

    ;; Set up the fast interrupt stack pointer.

    BIC     r0, r0, #Mode_Mask      ; Clear the mode bits
    ORR     r0, r0, #Mode_FIQ       ; Set FIR mode bits
    MSR     cpsr_c, r0              ; Change the mode
    ;LDR     sp, =__initial_fiqsp     ; End of FIQ_STACK
    LDR     sp, =||Image$$FIQ_STACK$$ZI$$Limit||     ; End of FIQ_STACK
    BIC     sp,sp,#0x7              ; Make sure SP is 8 aligned

    ;; Set up the normal stack pointer.

    BIC     r0 ,r0, #Mode_Mask       ; Clear the mode bits
    ORR     r0 ,r0, #Mode_SYS       ; Set System mode bits
    MSR     cpsr_c, r0              ; Change the mode
    ;LDR     sp, __initial_csp        ; End of CSTACK
    LDR     sp, =||Image$$ARM_LIB_STACK$$ZI$$Limit||       ; End of CSTACK
    BIC     sp,sp,#0x7              ; Make sure SP is 8 aligned
;===================================================================
; END: imported initialization from crt0_armcc.s - orig sample code
;===================================================================


;===================================================================
;===================================================================
; Begin new section of init, imported from DS-5 Base A5 project
;===================================================================
;===================================================================


;==================================================================
; Exception Vector Table
;==================================================================
; Note: LDR PC instructions are used here, though branch (B) instructions
; could also be used, unless the exception handlers are >32MB away.

Vectors
    LDR PC, Reset_Addr
    LDR PC, Undefined_Addr
    LDR PC, SVC_Addr
    LDR PC, Prefetch_Addr
    LDR PC, Abort_Addr
    LDR PC, Hypervisor_Addr
    LDR PC, IRQ_Addr
    LDR PC, FIQ_Addr

Reset_Addr      DCD     Reset_Handler
Undefined_Addr  DCD     Undefined_Handler
SVC_Addr        DCD     SVC_Handler
Prefetch_Addr   DCD     Prefetch_Handler
Abort_Addr      DCD     Abort_Handler
Hypervisor_Addr DCD     Hypervisor_Handler
IRQ_Addr        DCD     IRQ_Handler
FIQ_Addr        DCD     FIQ_Handler


;==================================================================
; Exception Handlers
;==================================================================

Undefined_Handler
    B   Undefined_Handler
SVC_Handler
    B   SVC_Handler
Prefetch_Handler
    B   Prefetch_Handler
Abort_Handler
    B   Abort_Handler
Hypervisor_Handler
    B   Hypervisor_Handler
IRQ_Handler
    B   IRQ_Handler
FIQ_Handler
    B   FIQ_Handler


;==================================================================
; Reset Handler
;==================================================================
Reset_Handler   FUNCTION {}

;==================================================================
; 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

; The MMU is enabled later, before calling main().  Caches and branch prediction are enabled inside main(),
; after the MMU has been enabled and scatterloading has been performed.

;==================================================================
; Initialize Supervisor Mode Stack
; Note stack must be 8 byte aligned.
;==================================================================

        IMPORT  ||Image$$ARM_LIB_STACK$$ZI$$Limit||   ; Linker symbol from scatter file
        LDR     SP, =||Image$$ARM_LIB_STACK$$ZI$$Limit||

;==================================================================
; Invalidate Data and Instruction TLBs and branch predictor
;==================================================================

        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 Vector Base Address Register (VBAR) to point to this application's vector table
;===================================================================

        LDR     r0, =Vectors
        MCR     p15, 0, r0, c12, c0, 0

;==================================================================
; Cache Invalidation code for Cortex-A7
; NOTE: Neither Caches, nor MMU, nor BTB need post-reset invalidation on Cortex-A7,
; but forcing a cache invalidation, makes the code more portable to other CPUs (e.g. Cortex-A9)
;==================================================================     
        ; Invalidate L1 Instruction Cache
        MRC     p15, 1, r0, c0, c0, 1      ; Read Cache Level ID Register (CLIDR)
        TST     r0, #0x3                   ; Harvard Cache?
        MOV     r0, #0                     ; SBZ
        MCRNE   p15, 0, r0, c7, c5, 0      ; ICIALLU - Invalidate instruction cache and flush branch target cache
        
        ; Invalidate Data/Unified Caches
        
        MRC     p15, 1, r0, c0, c0, 1      ; Read CLIDR
        ANDS    r3, r0, #0x07000000        ; Extract coherency level
        MOV     r3, r3, LSR #23            ; Total cache levels << 1
        BEQ     Finished                   ; If 0, no need to clean
    
        MOV     r10, #0                    ; R10 holds current cache level << 1
Loop1   ADD     r2, r10, r10, LSR #1       ; R2 holds cache "Set" position 
        MOV     r1, r0, LSR r2             ; Bottom 3 bits are the Cache-type for this level
        AND     r1, r1, #7                 ; Isolate those lower 3 bits
        CMP     r1, #2
        BLT     Skip                       ; No cache or only instruction cache at this level
        
        MCR     p15, 2, r10, c0, c0, 0     ; Write the Cache Size selection register
        ISB                                ; ISB to sync the change to the CacheSizeID reg
        MRC     p15, 1, r1, c0, c0, 0      ; Reads current Cache Size ID register
        AND     r2, r1, #7                 ; Extract the line length field
        ADD     r2, r2, #4                 ; Add 4 for the line length offset (log2 16 bytes)
        LDR     r4, =0x3FF
        ANDS    r4, r4, r1, LSR #3         ; R4 is the max number on the way size (right aligned)
        CLZ     r5, r4                     ; R5 is the bit position of the way size increment
        LDR     r7, =0x7FFF
        ANDS    r7, r7, r1, LSR #13        ; R7 is the max number of the index size (right aligned)

Loop2   MOV     r9, r4                     ; R9 working copy of the max way size (right aligned)

Loop3   ORR     r11, r10, r9, LSL r5       ; Factor in the Way number and cache number into R11
        ORR     r11, r11, r7, LSL r2       ; Factor in the Set number
        MCR     p15, 0, r11, c7, c6, 2     ; Invalidate by Set/Way
        SUBS    r9, r9, #1                 ; Decrement the Way number
        BGE     Loop3
        SUBS    r7, r7, #1                 ; Decrement the Set number
        BGE     Loop2
Skip    ADD     r10, r10, #2               ; increment the cache number
        CMP     r3, r10
        BGT     Loop1

Finished


;===================================================================
; Cortex-A5 MMU Configuration
; Set translation table base
;===================================================================

        IMPORT ||Image$$VECTORS$$Base||    ; From scatter file
        IMPORT ||Image$$TTB$$ZI$$Base||    ; From scatter file

        ; Cortex-A5 supports two translation tables
        ; Configure translation table base (TTB) control register cp15,c2
        ; to a value of all zeros, indicates we are using TTB register 0.

        MOV     r0,#0x0
        MCR     p15, 0, r0, c2, c0, 2

        ; write the address of our page table base to TTB register 0
        LDR     r0,=||Image$$TTB$$ZI$$Base||
        MOV     r1, #0x08                  ; RGN=b01  (outer cacheable write-back cached, write allocate)
                                           ; S=0      (translation table walk to non-shared memory)
        ORR     r1,r1,#0x40                ; IRGN=b01 (inner cacheability for the translation table walk is Write-back Write-allocate)
                                             
        ORR     r0,r0,r1                    
        MCR     p15, 0, r0, c2, c0, 0              


;===================================================================
; PAGE TABLE generation 

; Generate the page tables
; Build a flat translation table for the whole address space.
; ie: Create 4096 1MB sections from 0x000xxxxx to 0xFFFxxxxx


; 31                 20 19  18  17  16 15  14   12 11 10  9  8     5   4    3 2   1 0
; |section base address| 0  0  |nG| S |AP2|  TEX  |  AP | P | Domain | XN | C B | 1 0|
;
; Bits[31:20]   - Top 12 bits of VA is pointer into table
; nG[17]=0      - Non global, enables matching against ASID in the TLB when set.
; S[16]=0       - Indicates normal memory is shared when set.
; AP2[15]=0  
; AP[11:10]=11  - Configure for full read/write access in all modes
; TEX[14:12]=000
; CB[3:2]= 00   - Set attributes to Strongly-ordered memory.
;                 (except for the 0x600 descriptor, see below)
; IMPP[9]=0     - Ignored
; Domain[5:8]=1111   - Set all pages to use domain 15
; XN[4]=0       - Execute never disabled
; Bits[1:0]=10  - Indicate entry is a 1MB section
;===================================================================
		LDR     r0,=||Image$$TTB$$ZI$$Base||
        LDR     r1,=0xfff                   ; loop counter
        LDR     r2,=2_00000000000000000000110111100010

        ; r0 contains the address of the translation table base
        ; r1 is loop counter
        ; r2 is level1 descriptor (bits 19:0)

        ; use loop counter to create 4096 individual table entries.
        ; this writes from address 'Image$$TTB$$ZI$$Base' + 
        ; offset 0x3FFC down to offset 0x0 in word steps (4 bytes)

init_ttb_1
        ORR     r3, r2, r1, LSL#20          ; R3 now contains full level1 descriptor to write
        STR     r3, [r0, r1, LSL#2]         ; Str table entry at TTB base + loopcount*4
        SUBS    r1, r1, #1                  ; Decrement loop counter
        BPL     init_ttb_1

        ; In this example, the 1MB section based at '||Image$$VECTORS$$Base||' is setup specially as cacheable (write back mode).
        ; TEX[14:12]=001 and CB[3:2]= 11, Outer and inner write back, write allocate normal memory.
        LDR     r1,=||Image$$VECTORS$$Base|| ; Base physical address of code segment
        LSR     r1, #20                    ; Shift right to align to 1MB boundaries
        ORR     r3, r2, r1, LSL#20         ; Setup the initial level1 descriptor again
	
        ; set  Outer and Inner Write-Back, Write-Allocate
        ;ORR     r3,r3,#2_0000000001100     ; Set CB bits
        ;ORR     r3,r3,#2_1000000000000     ; Set TEX bit 12
	
	; set strongly-ordered
	ORR     r3,r3,#2_0000000000000     ; Set CB bits
        ORR     r3,r3,#2_0000000000000     ; Set TEX bit 12
        STR     r3, [r0, r1, LSL#2]        ; str table entry
   
;===================================================================        
; Setup domain control register - Enable all domains to client mode
;===================================================================

        MRC     p15, 0, r0, c3, c0, 0      ; Read Domain Access Control Register
        LDR     r0, =0x55555555            ; Initialize every domain entry to b01 (client)
        MCR     p15, 0, r0, c3, c0, 0      ; Write Domain Access Control Register          

    IF {TARGET_FEATURE_NEON} || {TARGET_FPU_VFP}
;==================================================================
; Enable access to NEON/VFP by enabling access to Coprocessors 10 and 11.
; Enables Full Access i.e. in both privileged and non privileged modes
;==================================================================

        MRC     p15, 0, r0, c1, c0, 2      ; Read Coprocessor Access Control Register (CPACR)
        ORR     r0, r0, #(0xF << 20)       ; Enable access to CP 10 & 11
        MCR     p15, 0, r0, c1, c0, 2      ; Write Coprocessor Access Control Register (CPACR)
        ISB

;==================================================================
; Switch on the VFP and NEON hardware
;=================================================================

        MOV     r0, #0x40000000
        VMSR    FPEXC, r0                   ; Write FPEXC register, EN bit set
    ENDIF

    
;===================================================================
; Enable MMU and branch to main_entry
; Leaving the caches disabled until after scatter loading.
;===================================================================

        IMPORT  main_entry                     ; Before MMU enabled import label to __main

        LDR     r12,=main_entry                ; Save this in register for possible long jump

        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, #0x2               ; Clear A bit  1 to disable strict alignment fault checking
        ;ORR     r0, r0, #0x1               ; Set M bit 0 to enable MMU before scatter loading
        MCR     p15, 0, r0, c1, c0, 0      ; Write CP15 System Control register

; Now the MMU is enabled, virtual to physical address translations will occur. This will affect the next
; instruction fetch.
;
; The two instructions currently in the ARM pipeline will have been fetched before the MMU was enabled.
; The branch to __main is safe because the Virtual Address (VA) is the same as the Physical Address (PA)
; (flat mapping) of this code that enables the MMU and performs the branch

       ;BL enable_caches

        BX      r12                        ; Branch to __main  C library entry point
        
        ENDFUNC



;==================================================================
; Enable caches and branch prediction
; This code must be run from a privileged mode
;==================================================================

        AREA   ENABLECACHES, CODE, READONLY

        EXPORT enable_caches

enable_caches  FUNCTION

;==================================================================
; Enable caches and branch prediction
;==================================================================

        MRC     p15, 0, r0, c1, c0, 0      ; Read System Control Register
        ORR     r0, r0, #(0x1 << 12)       ; Set I bit 12 to enable I Cache
        ORR     r0, r0, #(0x1 << 2)        ; Set C bit  2 to enable D Cache
        ORR     r0, r0, #(0x1 << 11)       ; Set Z bit 11 to enable branch prediction
        MCR     p15, 0, r0, c1, c0, 0      ; Write System Control Register

        BX      lr

        ENDFUNC


        END
