#*************************************************************************************************************************/
#* FILE NAME: ctr0_core2.s                      COPYRIGHT (c) Freescale 2012                                           */
#*                                                       All Rights Reserved                                                                   */
#* DESCRIPTION:                                                                                                                               */
#* This is the crt0 file for Core2(IOP) on the MPC5777M MCU                                                                 */
#* Flash Execution                                                           	                                                                 */
#*===========================================================================*/
#* REV      AUTHOR        DATE        DESCRIPTION OF CHANGE                  
#* ---   -----------    ----------    ---------------------                  
#* 1.0	  D.McMenamin    13/Sept/12   Initial Public Release                 
#**************************************************************************************************************************/

 .globl	_start_core2

# 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
 
#**************************** .bh Boot Header ********************************/	
 .section .bh 
 .LONG 0x005A0001                # BH conf: IOP Only - Can boot others via ME
#.LONG 0x005A000F                # BH conf: All cores ON inc SC
 .LONG _start_core2              # IOP Reset Vector
 .LONG 0x00000000                # Configuration Bits
 .LONG 0x00000000                # Configuration Bits
 .LONG 0x01080000                # CPU0 Reset Vector
 .LONG 0x01040000                # CPU1 Reset Vector
 .LONG 0x01080000                # LSCPU Reset Vector
 .LONG 0x00000000                # Padding
 
#************************** GHS Section Infomation ***************************/	
 .section .init , axv
 .vle
 .align 4
_start_core2:

#************************ Configure Flash Access Timing **********************/
 e_lis       r5, __SRAM_BASE_ADDR@h          # Load Address of SRAM to r5
 e_or2i      r5, __SRAM_BASE_ADDR@l

# Write start of SRAM to init ECC    
# Write all 32 GPR registers to SRAM
e_stmw      r0,0(r5)            

# Branch to function that will copy procedure to RAM 
e_b copy_to_ram

# This write the reguster to configure the flash timing
# Will be executed from RAM 
reduce_flash_ws:
 e_lis    r3, 0x0000 # load register settings to r3 
 e_or2i   r3, 0x4500
 e_lis    r4, 0xFC03 # load address of PCR1 to r4
 e_or2i   r4, 0x0000
 e_stw   r3, 0(r4)   # store settings to PCR1 reg
 se_blr              # Return to program in flash

copy_to_ram:
# load start address of reduce_flash_ws to r3
 e_lis r3, reduce_flash_ws@h   
 e_or2i r3, reduce_flash_ws@l
# Load Start address of copy_to_ram to r4 
 e_lis r4, copy_to_ram@h       
 e_or2i r4, copy_to_ram@l
# Calculate procedure size and load counter
  subf r4, r3, r4 
  mtctr r4

 mtlr r5 # load code start address to link reg 

copy:
# copy reduce_flash_ws to RAM 
  e_lbz r6, 0(r3)
  e_stb r6, 0(r5)
  e_addi r3, r3, 1
  e_addi r5, r5, 1
  e_bdnz copy
# branch to reduce_flash_ws in RAM
  se_blrl

#******************************** Turn off SWTs ******************************/
#SWT2
 e_lis   r4, 0xFC05
 e_or2i  r4, 0x8000

 e_li    r3, 0xC520
 e_stw   r3, 0x10(r4)

 e_li    r3, 0xD928
 e_stw   r3, 0x10(r4)

 e_lis   r3, 0x7F00
 e_or2i  r3, 0x010A
 e_stw   r3, 0(r4)

#SWT1
 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, 0x7F00
 e_or2i  r3, 0x010A
 e_stw   r3, 0(r4)

#SWT0
 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, 0x7F00
 e_or2i  r3, 0x010A
 e_stw   r3, 0(r4)

#********************************* Enable BTB ********************************/  
# Flush & Enable BTB - Set BBFI bit in BUCSR
  e_li r3, 0x201
  mtspr 1013, r3
  se_isync
  
#**************************** Init Core Registers ****************************/ 
# The E200Z4 core needs its registers initialising before they are used 
# otherwise in Lock Step mode the two cores will contain different random data.
# If this is stored to memory (e.g. stacked) it will cause a Lock Step error.

# GPR's 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 any other CPU register which might be stacked (before being used).

  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
  mtspr 8,r31          #LR
  


#***************************** Initialise SRAM ECC ***************************/
# Store number of 128Byte (32GPRs) segments in Counter
 e_lis       r5, __SRAM_SIZE@h  # Initialize r5 to size of SRAM (Bytes)
 e_or2i      r5, __SRAM_SIZE@l
 e_srwi      r5, r5, 0x7         # Divide SRAM size by 128
 mtctr       r5                  # Move to counter for use with "bdnz"
    
# Base Address of the internal SRAM
 e_lis       r5, __SRAM_BASE_ADDR@h
 e_or2i      r5, __SRAM_BASE_ADDR@l

# Fill SRAM with writes of 32GPRs    
sram_loop:
    e_stmw      r0,0(r5)            # Write all 32 registers to SRAM
    e_addi      r5,r5,128           # Increment the RAM pointer to next 128bytes
    e_bdnz      sram_loop           # Loop for all of SRAM

#************************ Initialise Local Data SRAM *************************/
# Store number of 128Byte (32GPRs) segments in Counter
 e_lis       r5, __LOCAL_DMEM_SIZE@h  # Initialize r5 to size of SRAM (Bytes)
 e_or2i      r5, __LOCAL_DMEM_SIZE@l
 e_srwi      r5, r5, 0x7         # Divide SRAM size by 128
 mtctr       r5                  # Move to counter for use with "bdnz"
    
# Base Address of the Local SRAM
 e_lis       r5, __LOCAL_DMEM_BASE_ADDR@h
 e_or2i      r5, __LOCAL_DMEM_BASE_ADDR@l

# Fill Local SRAM with writes of 32GPRs    
ldmem_loop:
    e_stmw      r0,0(r5)            # Write all 32 registers to SRAM
    e_addi      r5,r5,128           # Increment the RAM pointer to next 128bytes
    e_bdnz      ldmem_loop           # Loop for all of SRAM

#************************ Initialise Local Instruction SRAM ******************/
# Store number of 128Byte (32GPRs) segments in Counter
 e_lis       r5, __LOCAL_IMEM_SIZE@h  # Initialize r5 to size of SRAM (Bytes)
 e_or2i      r5, __LOCAL_IMEM_SIZE@l
 e_srwi      r5, r5, 0x7         # Divide SRAM size by 128
 mtctr       r5                  # Move to counter for use with "bdnz"
    
# Base Address of the Local SRAM
 e_lis       r5, __LOCAL_IMEM_BASE_ADDR@h
 e_or2i      r5, __LOCAL_IMEM_BASE_ADDR@l

# Fill Local SRAM with writes of 32GPRs    
limem_loop:
    e_stmw      r0,0(r5)            # Write all 32 registers to SRAM
    e_addi      r5,r5,128           # Increment the RAM pointer to next 128bytes
    e_bdnz      limem_loop          # Loop for all of SRAM	       
		   
#*************************** Enable ME Bit in MSR *****************************/
# mfmsr r6
# e_or2i r6,0x1000
# mtmsr r6

#****************** Invalidate and Enable the 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  
		   
#*************** Load Initialised Data Values from Flash into RAM ************/
# Initialised Data - ".data"
DATACOPY:
    e_lis       r9, __DATA_SIZE@ha      # Load upper SRAM load size (# of bytes) into R9
    e_or2i      r9, __DATA_SIZE@l       # Load lower SRAM load size into R9                                     
    e_cmp16i    r9,0                    # Compare to see if equal to 0                                  
    e_beq       SDATACOPY               # Exit cfg_ROMCPY if size is zero (no data to initialise)
                                        
    mtctr       r9                      # Store no. of bytes to be moved in counter
                                        
    e_lis       r10, __DATA_ROM_ADDR@h  # Load address of first SRAM load into R10
    e_or2i      r10, __DATA_ROM_ADDR@l  # Load lower address of SRAM load into R10
    e_subi      r10,r10, 1              # Decrement address to prepare for ROMCPYLOOP

    e_lis       r5, __DATA_SRAM_ADDR@h  # Load upper SRAM address into R5 (from linker file)
    e_or2i      r5, __DATA_SRAM_ADDR@l  # Load lower SRAM address into R5 (from linker file)
    e_subi      r5, r5, 1               # Decrement address to prepare for ROMCPYLOOP

DATACPYLOOP:
    e_lbzu      r4, 1(r10)              # Load data byte at R10 into R4,incrementing (update) ROM address
    e_stbu      r4, 1(r5)               # Store R4 data byte into SRAM at R5 and update SRAM address 
    e_bdnz      DATACPYLOOP             # Branch if more bytes to load from ROM

# Small Initialised Data - ".sdata" 
SDATACOPY:    
    e_lis       r9, __SDATA_SIZE@ha     # Load upper SRAM load size (# of bytes) into R9
    e_or2i      r9, __SDATA_SIZE@l      # Load lower SRAM load size into R9                                     
    e_cmp16i    r9,0                    # Compare to see if equal to 0                                  
    e_beq       ROMCPYEND               # Exit cfg_ROMCPY if size is zero (no data to initialise)
                                        
    mtctr       r9                      # Store no. of bytes to be moved in counter
                                        
    e_lis       r10, __SDATA_ROM_ADDR@h # Load address of first SRAM load into R10
    e_or2i      r10, __SDATA_ROM_ADDR@l # Load lower address of SRAM load into R10
    e_subi      r10,r10, 1              # Decrement address to prepare for ROMCPYLOOP

    e_lis       r5, __SDATA_SRAM_ADDR@h # Load upper SRAM address into R5 (from linker file)
    e_or2i      r5, __SDATA_SRAM_ADDR@l # Load lower SRAM address into R5 (from linker file)
    e_subi      r5, r5, 1               # Decrement address to prepare for ROMCPYLOOP

SDATACPYLOOP:
    e_lbzu      r4, 1(r10)              # Load data byte at R10 into R4,incrementing (update) ROM address
    e_stbu      r4, 1(r5)               # Store R4 data byte into SRAM at R5 and update SRAM address 
    e_bdnz      SDATACPYLOOP            # Branch if more bytes to load from ROM
    
    
ROMCPYEND:

#****************************** Configure Stack ******************************/
 e_lis       r1, __SP_INIT@h     # Initialize stack pointer r1 to
 e_or2i      r1, __SP_INIT@l     # value in linker command file. 

 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).

 e_stwu      r0,-64(r1)          # Terminate stack.


# Jump to GHS start code
 e_bl _start

# Jump to Main
 e_bl main
	  
       

		
