#******************************************************************************
# FILE NAME     :  crt0_core0_flash.s                                         *
# DESCRIPTION   :  crt0 file for core 0                                       *
# DATE          :  July 17, 2013                                              *
#                                                                             *
#******************************************************************************

.global		_start_core0
.extern		main_core0			         # main routine


# macro to allow immediate register load to be done more easily
 		.macro 	e_lwi  register value
  				e_lis register, value@h
  				e_or2i register, value@l        
  		.endm

.section    .init_core0 , axv
.vle

#******************************************************************************
# Function: _start_core0                                                      * 
# Content:  Initialization of Core0                                           * 
#******************************************************************************
_start_core0:	

#**************************************************
#     Initialize Core Registers            
#**************************************************
# The e200z425 core registers must be initialized before they are used.
# otherwise in Lock Step mode the two core regisers will contain different 
# random data. If this is stored to memory (e.g. stacked) it will cause a
# Lock Step error.

# Init GPRs 0-31
    e_li   r0, 0
    e_li   r1, 0  
    e_li   r2, 0
    e_li   r3, 0
    e_li   r4, 0
    e_li   r5, 0
    e_li   r6, 0
    e_li   r7, 0
    e_li   r8, 0
    e_li   r9, 0
    e_li   r10, 0
    e_li   r11, 0
    e_li   r12, 0
    e_li   r13, 0
    e_li   r14, 0
    e_li   r15, 0
    e_li   r16, 0
    e_li   r17, 0
    e_li   r18, 0
    e_li   r19, 0
    e_li   r20, 0
    e_li   r21, 0
    e_li   r22, 0
    e_li   r23, 0
    e_li   r24, 0
    e_li   r25, 0
    e_li   r26, 0
    e_li   r27, 0
    e_li   r28, 0
    e_li   r29, 0
    e_li   r30, 0
    e_li   r31, 0

# Init other CPU registers
    mtspr 1,r1           #XER
    mtcrf 0xFF, r1      
    mtspr CTR,  r1
    mtspr SPRG0, r1
    mtspr SPRG1, r1
    mtspr SPRG2, r1
    mtspr SPRG3, r1 
    mtspr SRR0, r1
    mtspr SRR1, r1
    mtspr CSRR0, r1
    mtspr CSRR1, r1
    mtspr MCSRR0, r1
    mtspr MCSRR1, r1
    mtspr DEAR, r1
    mtspr IVPR, r1
    mtspr USPRG0, r1
    mtspr 62, r1         #ESR

#**************************************************
#     Disable EE/ME/CE                            *
#     MSR[ME] = 1, others = 0                     *
#**************************************************
    e_lis   r6, 0x0000
    e_or2i  r6, 0x1000
    mtmsr r6

#**************************************************
#     Enable Instruction Cache                    *
#**************************************************
__icache_cfg:
# Set ICINV bit in L1CSR1 Register
    e_li    r5, 0x2
    mtspr   1011,r5
    e_li    r7, 0x4
    e_li    r8, 0x2
    e_lwi   r11, 0xFFFFFFFB

# Invalidate cache before enabling
__icache_inv:
    mfspr   r9, 1011        #Read L1CSR1 register to r9
    and.    r10, r7, r9     #Check if ICABT(Aborted) is set
    e_beq   __icache_no_abort
# if abort detected, clear ICABT and re-run invalidation
    and.    r10, r11, r9
    mtspr   1011, r10
    e_b __icache_cfg

__icache_no_abort:
# Check that invalidation has completed(ICINV = 0)
    and.    r10, r8, r9
    e_bne   __icache_inv    #Branch if invalidation not complete

#Enable ICache by setting ICE bit in the L1CSR register
    mfspr   r5, 1011
    e_ori   r5, r5, 0x0001
    se_isync
    msync
    mtspr   1011, r5

#**************************************************
#     Enable Branch Target Buffer                 *
#**************************************************
# Flush BTB - Set BBFI bit in BUCSR
    e_li r3, 0x200
    mtspr 1013, r3

# Enable BTB - Set BPEN bit in BUCSR        
    se_li r3, 0x1
    mtspr 1013, r3

#**************************************************
#     Initialize DMEM for stack                   *
#**************************************************
#Clear R16 to R31
    e_li   r16, 0x0
    e_li   r17, 0x0
    e_li   r18, 0x0
    e_li   r19, 0x0
    e_li   r20, 0x0
    e_li   r21, 0x0
    e_li   r22, 0x0
    e_li   r23, 0x0
    e_li   r24, 0x0
    e_li   r25, 0x0
    e_li   r26, 0x0
    e_li   r27, 0x0
    e_li   r28, 0x0
    e_li   r29, 0x0
    e_li   r30, 0x0
    e_li   r31, 0x0

# Store number of 64Byte (16GPRs) segments in Counter
    e_lis   r5, __STACK_SIZE@h           # Initialize r5 with stack size 
    e_or2i  r5, __STACK_SIZE@l     
    e_srwi  r5, r5, 0x6                  # Divide stack size by 64
    mtctr   r5                           # Move to counter for use with "bdnz"

# Base Address of the stack
    e_lis   r5, (__STACK_START_CPU0@h)   # initialize stack pointer r5
    e_or2i  r5, (__STACK_START_CPU0@l)   # with value provided by linker

# Fill Local SRAM with writes of 16GPRs    
lsram_loop_cpu0:
    e_stmw  r16,0(r5)                    # Write value of r16-r31 to SRAM
    e_addi  r5,r5,64                     # Increment the RAM pointer
    e_bdnz  lsram_loop_cpu0              # Loop for all of SRAM

#**************************************************
#     Initialize IMEM/DMEM                        *
#**************************************************
#Clear R16 to R31
    e_li   r16, 0x0
    e_li   r17, 0x0
    e_li   r18, 0x0
    e_li   r19, 0x0
    e_li   r20, 0x0
    e_li   r21, 0x0
    e_li   r22, 0x0
    e_li   r23, 0x0
    e_li   r24, 0x0
    e_li   r25, 0x0
    e_li   r26, 0x0
    e_li   r27, 0x0
    e_li   r28, 0x0
    e_li   r29, 0x0
    e_li   r30, 0x0
    e_li   r31, 0x0

# Init start and end address  for initialize IMEM
    e_lis    r2, __IMEM_START_CPU0@h     # R2 <- start address of IMEM  
    e_or2i   r2, __IMEM_START_CPU0@l     
    e_lis    r3, __IMEM_END_CPU0@h       # R3 <- end address of IMEM  
    e_or2i   r3, __IMEM_END_CPU0@l       

# Copy register value to IMEM
LOOP_IMEM_INIT0:
    e_stmw  r16, 0(r2)                   # Write IMEM region with zero
    e_addi  r2,r2,64                     # Increment the IMEM pointer
    cmpl    0x0, 0x0, r2, r3             # Check if it is end of address
    e_bne   LOOP_IMEM_INIT0              # Loop if not end of address

# Init start and end address  for initialize DMEM
    e_lis    r2, __DMEM_START_CPU0@h     # R2 <- start address of DMEM  
    e_or2i   r2, __DMEM_START_CPU0@l
    e_lis    r3, __DMEM_END_CPU0@h       # R3 <- end address of DMEM  
    e_or2i   r3, __DMEM_END_CPU0@l      

# init DMEM 
LOOP_DMEM_INIT0:
    e_stmw  r16, 0(r2)                   # Write DMEM region with zero
    e_addi  r2,r2,64                     # Increment the DMEM pointer
    cmpl    0x0, 0x0, r2, r3             # Check if it is end of address
    e_bne   LOOP_DMEM_INIT0              # Loop if not end of address

#**************************************************
#     Initialize stack  address                   *
#**************************************************
    e_lis   r1, (__STACK_END_CPU0@h)-16  # initialize stack pointer r1
    e_or2i  r1, (__STACK_END_CPU0@l)-16  # with value provided by linker

#**************************************************
#     Set SDA address to r13 and r2               *
#**************************************************
	e_lis	r13, _SDA_BASE_@h            # Initialize r13 to sdata base
	e_or2i	r13, _SDA_BASE_@l            # (provided by linker).
	e_lis	r2, _SDA2_BASE_@h            # Initialize r2 to sdata2 base
	e_or2i	r2, _SDA2_BASE_@l            # (provided by linker).

#**************************************************
#     Setup IVPR                                  *
#**************************************************              
    e_lis r5, __IVPR_CORE0@h   
    e_or2i r5, __IVPR_CORE0@l
    mtIVPR r5 
  
#**************************************************
#     Jump to Main                                *
#**************************************************
	e_b	main_core0

loop:
    se_b loop

