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

.global		_start_core1

.extern		Clock_and_Mode_Init          # Peripheral and clock configuration
.extern		main			     # 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

#************************** .boot_header section  *****************************	
    .section .boot_header 
	.LONG 0x005A0008	# BH conf: Core1 Only - Can boot others via ME
	.LONG 0x00000000	# Reserved for future use
	.LONG 0x00000000        # Configuration Bits
	.LONG 0x00000000        # Configuration Bits
	.LONG 0x01040000        # CPU Checker Reset Vector
	.LONG 0x01000020        # CPU1 Reset Vector
	.LONG 0x01040000        # CPU0 Reset Vector
	.LONG 0x00000000        # Padding

#*****************************************************************************/	

.section    .init_core1 , "axv"
.vle

#******************************************************************************
# Function: _start_core1                                                      * 
# Content:  Initialization of Core1                                           * 
#******************************************************************************
_start_core1:

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

#**************************************************
#     Setup Flash/SRAM wait cycle                 *
#**************************************************
# This is the code copied to I-MEM
    e_bl	_Flash_WS_config
    
# This is the code copied to I-MEM
reduce_flash_ws:
    #RWSC=5, APC=2  for 200MHz
    #Enable Data Prefetch, Instruction Prefetch but disable Line Read Buffers
    e_lis   r3, 0x0000
    e_or2i  r3, 0x4554

    #Enable Data Prefetch, Instruction Prefetch but disable Line Read Buffers
    e_lis   r4, 0x0000
    e_or2i  r4, 0x0054

    #PFLASH_PFCR1 address
    e_lis   r5, 0xFC03
    e_or2i  r5, 0x0000

    #PFLASH_PFCR2 address
    e_lis   r6, 0xFC03
    e_or2i  r6, 0x0004

    #Configure flash
    e_stw   r3, 0(r5)
    e_stw   r4, 0(r6)

    #RWSC=5, APC=2  for 200MHz
    #Enable Line Read Buffers to flush buffers
    e_lis   r3, 0x0000
    e_or2i  r3, 0x4555

    #Enable Line Read Buffers to flush buffers
    e_lis   r4, 0x0000
    e_or2i  r4, 0x0055

    #Configure flash again to flush&enable buffers
    e_stw   r3, 0(r5)
    e_stw   r4, 0(r6)
    se_blr

# Configure 
_Flash_WS_config:

# load I-MEM start address
    e_lis   r5, 0x5100
    e_or2i  r5, 0x0000

# Initialize I-MEM
    e_stmw  r0,0(r5) 

# backup current link register to r9
    mfspr   r9, 8

# Set link register to I-MEM start address
    mtlr    r5

# Prepare addresses to copy instructions to IMEM 
    e_lis   r3, reduce_flash_ws@h
    e_or2i  r3, reduce_flash_ws@l
    e_lis   r4, _Flash_WS_config@h
    e_or2i  r4, _Flash_WS_config@l
    subf    r4, r3, r4
    mtctr   r4

# Copy to IMEM
copy:
    e_lbz   r6, 0(r3)
    e_stb   r6, 0(r5)
    e_addi  r3, r3, 1
    e_addi  r5, r5, 1
    e_bdnz  copy
    
# Jump to I-MEM start address
    se_blrl

# Restore link register from r9 to LR and return
    mtspr   8, r9

#**************************************************
#     Enable Instruction Cache                    *
#**************************************************
__icache_cfg:
    e_li    r5, 0x2
    mtspr   1011,r5
    
    e_li    r7, 0x4
    e_li    r8, 0x2
    e_lwi   r11, 0xFFFFFFFB

__icache_inv:
    mfspr   r9, 1011
    and.    r10, r7, r9
    e_beq   __icache_no_abort
    and.    r10, r11, r9
    mtspr   1011, r10
    e_b __icache_cfg

__icache_no_abort:
    and.    r10, r8, r9
    e_bne   __icache_inv

    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

#**************************************************
#     Disable watchdog timer                      *
#**************************************************
# Disable SWT0 -
# R4 holds base address of SWT0 (0xFC050000)
    e_lis   r4, 0xFC05
    e_or2i  r4, 0x0000
    e_li    r3, 0xC520
    e_stw   r3, 0x10(r4)
    e_li    r3, 0xD928
    e_stw   r3, 0x10(r4)
    e_lis   r3, 0xFF00
    e_or2i  r3, 0x010A
    e_stw   r3, 0(r4)

# Disable SWT1
# R4 holds base address of SWT1 (0xFC054000)
    e_lis   r4, 0xFC05
    e_or2i  r4, 0x4000
    e_li    r3, 0xC520
    e_stw   r3, 0x10(r4)
    e_li    r3, 0xD928
    e_stw   r3, 0x10(r4)
    e_lis   r3, 0xFF00
    e_or2i  r3, 0x010A
    e_stw   r3, 0(r4)

# Disable SWT3
# R4 holds base address of SWT3 (0xFC05C000)
    e_lis   r4, 0xFC05
    e_or2i  r4, 0xC000
    e_li    r3, 0xC520
    e_stw   r3, 0x10(r4)
    e_li    r3, 0xD928
    e_stw   r3, 0x10(r4)
    e_lis   r3, 0xFF00
    e_or2i  r3, 0x010A
    e_stw   r3, 0(r4)

#**************************************************
#     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 SRAM size by 64
    mtctr   r5                           # Move to counter for use with "bdnz"
    
# Base Address of the Local SRAM
    e_lis   r5, (__STACK_START_CPU1@h)   # initialize stack pointer r5
    e_or2i  r5, (__STACK_START_CPU1@l)   # with value provided by linker

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


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


#**************************************************
#     Initialize SRAM                             *
#**************************************************
# Init r2 and r3 with SRAM start and end address
    e_lis    r2, __ghs_ramstart@h        # R2 <- start address of SRAM  
    e_or2i   r2, __ghs_ramstart@l        
    e_lis    r3, __ghs_ramend@h          # R3 <- end address of SRAM
    e_or2i   r3, __ghs_ramend@l          

# 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

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

#**************************************************
#     Initialize initialized data                 *
#**************************************************
# Init start and end address  for DATA in Flash and SRAM/DMEM
    e_lis    r5, __DATA_SIZE_CPU1@h      # Initialize r5 with stack size 
    e_or2i   r5, __DATA_SIZE_CPU1@l       

    e_cmp16i r5,0                        # Compare to see if equal to 0                                  
    e_beq SDATACOPY1                     # Exit if size is zero (no data to initialise)
    e_srwi   r5, r5, 0x2                 # Divide SRAM size by 4 byte
    mtctr    r5                          # Move to counter for use with "bdnz"

    e_lis    r2, __DATA_ROM_ADDR_CPU1@h  # R2 <- start address of DATA ROM  
    e_or2i   r2, __DATA_ROM_ADDR_CPU1@l  
    e_lis    r3, __DATA_SRAM_ADDR_CPU1@h # R3 <- start address of DATA RAM   
    e_or2i   r3, __DATA_SRAM_ADDR_CPU1@l 

# Copy data value from ROM DATA to RAM DATA
LOOP_DATA_INIT1:
    e_lwz   r4, 0(r2)
    e_stwu  r4, 0(r3)
    e_addi  r2,r2,4                      # Increment the DATA ROM pointer to next 4bytes
    e_addi  r3,r3,4                      # Increment the DATA SRAM pointer to next 4bytes
    e_bdnz  LOOP_DATA_INIT1              # Loop for all of global data

SDATACOPY1:
# Init start and end address  for DATA in Flash and SRAM/DMEM
    e_lis    r5, __SDATA_SIZE_CPU1@h     # Initialize r5 with stack size 
    e_or2i   r5, __SDATA_SIZE_CPU1@l       
    e_cmp16i r5,0                        # Compare to see if equal to 0                                  
    e_beq end_of_global_init             # Exit if size is zero (no data to initialise)
    e_srwi   r5, r5, 0x2                 # Divide SRAM size by 4 byte
    mtctr    r5                          # Move to counter for use with "bdnz"

    e_lis    r2, __SDATA_ROM_ADDR_CPU1@h   # R2 <- start address of DATA ROM  
    e_or2i   r2, __SDATA_ROM_ADDR_CPU1@l   
    e_lis    r3, __SDATA_SRAM_ADDR_CPU1@h  # R3 <- start address of DATA RAM   
    e_or2i   r3, __SDATA_SRAM_ADDR_CPU1@l  

# Copy data value from ROM SDATA to RAM SDATA
LOOP_SDATA_INIT1:
    e_lwz   r4, 0(r2)
    e_stwu  r4, 0(r3)
    e_addi  r2,r2,4                      # Increment the DATA ROM pointer
    e_addi  r3,r3,4                      # Increment the DATA SRAM pointer 
    e_bdnz  LOOP_SDATA_INIT1             # Loop for all of global data

end_of_global_init:

#**************************************************
#     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_CPU1@h     # R2 <- start address of IMEM  
    e_or2i   r2, __IMEM_START_CPU1@l          
    e_lis    r3, __IMEM_END_CPU1@h       # R3 <- end address of IMEM  
    e_or2i   r3, __IMEM_END_CPU1@l       

# Copy register value to IMEM
LOOP_IMEM_INIT1:
    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_INIT1              # Loop if not end of address

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

# Copy register value to DMEM
LOOP_DMEM_INIT1:
    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_INIT1              # Loop if not end of address

#**************************************************
#     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_CORE1@h   
    e_or2i r5, __IVPR_CORE1@l
    mtIVPR r5 

#**************************************************
#     Enable peripheral and Clock Setup           *
#**************************************************
    e_bl    Clock_and_Mode_Init

#**************************************************
#     Jump to Main                                *
#**************************************************
	e_b	main

loop:
	se_b	loop
	  
