#*****************************************************************************/
#* FILE NAME: ctr0_core2.s                      COPYRIGHT (c) Freescale 2012 */
#*                                                       All Rights Reserved */
#* DESCRIPTION:                                                              */
#* This is the crt0 file for Core2(IOP) on the MPC5764M MCU                  */
#* RAM Execution                                                             */	
#*===========================================================================*/
#* REV      AUTHOR        DATE        DESCRIPTION OF CHANGE                  */
#* ---   -----------    ----------    ---------------------                  */
#* 0.1	  D.McMenamin    15/Jun/12    Initial Version                        */
#*****************************************************************************/

 .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

#************************** GHS Section Infomation ***************************/	 
 .section .init , axv
 .vle
 
_start_core2:

#****************************** 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
  

#*************************** 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

#****************************** 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 Main
 e_bl main
	  
       

		
