

#include <stdio.h>


#define Mode_Bits     0x1F
#define Irq_Bits      0x80
#define Mode_USR_32   0x10
#define Mode_FIQ_32   0x11
#define Mode_IRQ_32   0x12
#define Mode_SVC_32   0x13
#define Mode_ABT_32   0x17
#define Mode_UND_32   0x1B
#define Mode_SYS_32   0x1F
#define Mode_SE_32    0x16

#define    SVC_ACTN_SETUP_STACKS       0x10
#define    SVC_ACTN_ENABLE_IRQ         0x11
#define    SVC_ACTN_SETUP_NSEC_VBAR    0x12
#define    SVC_ACTN_GOTO_SVC_MODE      0x13
#define    SVC_ACTN_GOTO_USR_MODE      0x14
#define    SVC_ACTN_START_CYC_CNTR     0x15
#define    SVC_ACTN_ENABLE_ICACHE      0x16

#define 	SMS_MPAXL_0_0		0x200	// PrivID0 SMS MPAX registers
#define 	SMS_MPAXH_0_0		0x204
#define 	SES_MPAXL_0_0		0x600	// PrivID0 SES MPAX registers
#define 	SES_MPAXH_0_0		0x604

#define 	asm 	__asm__ __volatile__

/* ----------------------------------------------------------------------------
* TYPE: UWORD64, WORD64
*
* DESCRIPTION:  64 bit types
*
* -----------------------------------------------------------------------------
*/
typedef unsigned long long  UWORD64;
typedef          long long  WORD64;


/* ----------------------------------------------------------------------------
 * TYPE: REG_UWORD64, REG_WORD64
 *
 * DESCRIPTION: 64 bit register types.
 *
 * -----------------------------------------------------------------------------
 */
typedef volatile unsigned long  long  REG_UWORD64;
typedef volatile          long  long  REG_WORD64;


/* ----------------------------------------------------------------------------
* MACRO: RD_MEM_64, WR_MEM_64
*
* DESCRIPTION:  64 bit register access macros
*
* -----------------------------------------------------------------------------
*/
#define RD_MEM_64(addr)	         ((UWORD64)(*((REG_UWORD64 *)(addr))))
#define WR_MEM_64(addr, data)	 (*((REG_UWORD64 *)(addr)) = (UWORD64)(data))

#define 	msmc_cfg_base	(0x0BC00000)
#define mkptr(base,offset)	((volatile unsigned int *)((base)+(offset)))

//volatile unsigned int TTB_LOCATION;

unsigned int read_cpu_id(void)
{
	unsigned int cpuid = 0;

	asm ("MRC p15, 0, %0, c0, c0, 5" : "=r" (cpuid)); // read Multiprocess Affinity Register

	cpuid &= 0x3;	//extract CPU ID bits

	return (cpuid);
}

void goto_user_mode(void)
{
	asm ("stmfd   sp!,{r0-r4}");      	//PUSH
	asm ("mov r0, r14");      	//PUSH
	asm ("mrs r5, CPSR"); 				//Take a copy of Program Status Register
	asm ("bic r5, r5, #0x1f"); 			//Clear the mode bits
	asm ("orr r5, r5, #0x10"); 			//Set User mode
	asm ("msr CPSR_cxsf, r5");			//Write back the modified value
	asm ("mov r14, r0");      	//PUSH
	asm ("ldmfd   sp!,{r0-r4}");       //POP
}

void arm_enable_irq (void)
{
	asm ("stmfd   sp!,{r0-r4}");   // PUSH
	//asm ("cpsie i");				// Enable IRQ

    //Enable IRQ in USR mode
	asm ("mrs     r4, CPSR");      	//Take a copy of Program Status Register
	asm ("bic     r4,r4,#0x80");  	//Clear the Irq bits
	asm ("msr     CPSR_cxsf, r4");  	//Write back the modified SPS

	asm ("ldmfd   sp!,{r0-r4}");   // POP

}

void svc_intr_isr(unsigned int svcCode)
{
    switch (svcCode)
    {
#if 0
        case SVC_ACTN_SETUP_STACKS:
            arm_setup_stacks();
            break;
#endif

        case SVC_ACTN_ENABLE_IRQ:
            arm_enable_irq();
            break;

        case SVC_ACTN_SETUP_NSEC_VBAR:
            //arm_init_vbar_nsec();
            break;

        case SVC_ACTN_GOTO_SVC_MODE:
            //arm_goto_mode_supervisor();
            break;

        case SVC_ACTN_GOTO_USR_MODE:
            //arm_goto_mode_user();
            break;

        case SVC_ACTN_START_CYC_CNTR:
            //arm_enable_ccntr();
            break;

        case SVC_ACTN_ENABLE_ICACHE:
            //arm_enable_ICache();
            break;

        default:
            break;
    }
}


void arm_disable_cache (void)
{
	asm ("MRC    p15, 0, r0, c1, c0, 0");       // Read CP15 register 1 into r0
	asm ("BIC    r0, r0, #(1 << 2)");           // Disable D-cache
	asm ("BIC    r0, r0, #(1 << 12)");          // Disable I-cache
	asm ("MCR    p15, 0, r0, c1, c0, 0");       // Write value back
	
}

void arm_enable_cache (void)
{
	
	asm ("MRC    p15, 0, r0, c1, c0, 0");       // Read CP15 register 1 into r0
	asm ("ORR    r0, r0, #(1 << 2)");           // Enable D-cache
	asm ("ORR    r0, r0, #(1 << 12)");          // Enable I-cache
	asm ("MCR    p15, 0, r0, c1, c0, 0");       // Write value back
	asm ("ISB");
	

}

void arm_enable_mmu (void)
{
	asm ("MRC    p15, 0, r0, c1, c0, 0");       // Read CP15 register 1 into r0
	asm ("ORR    r0, r0, #(1 << 0)");           // Enable MMU
	asm ("ORR    r0, r0, #(1 << 11)");          // Enable branch prediction
	asm ("MCR    p15, 0, r0, c1, c0, 0");       // Write value back
	asm ("ISB");

}


void flush_tlb (void)
{
    
    asm ("MOV     r0, #0");
    asm ("MCR     p15, 0, r0, c8, c7, 0");     //flush Translation look-aside buffer
    asm ("ISB");
    

}


void lpae_mmu_enable (unsigned int table_base_lpae)
{
	
	asm ("MOV  r8, r0");
	asm ("MOV  r0, #0x80000000");
	asm ("MCR  p15, 0, r0, c2, c0, 2");
	asm ("ISB");
	asm ("LDR  r0, =0x0004FF44");
	asm ("MCR  p15,0, r0, c10, c2, 0");
	asm ("ISB");
	asm ("LDR  r0, =0x00ccFF88");
	asm ("MCR  p15,0, r0, c10, c2, 1");
	asm ("ISB");
	asm ("MOV  r1, #0x0");  //Bits 63:32.  Example only uses bottom 4GB, so top address bits always 0.
	asm ("MOV  r0, r8");
	asm ("MCRR p15, 0, r0, r1, c2"); 	//Write 64-bit TTBR0
	asm ("ISB");
	asm ("MRC  p15, 0, r0, c1, c0, 0");       //Read current control reg
	asm ("ORR  r0, r0, #0x01");               //Set M bit
	asm ("MCR  p15, 0, r0, c1, c0, 0");       //Write reg back

	//data_copy();
	//asm ("ISB");

}


void cache_ctlr_setup (void)
{
        //-----------------------------
        //Setup RL; per tetris specbook
        //-----------------------------
        asm ("MRC    p15, 1, r0, c9, c0, 2");
        asm ("ORR    r0, r0, #(0x2 << 10)");  //DRS      (2 S)
        asm ("ORR    r0, r0, #(0x1 << 6)");	  //TRL      (2 c)
        asm ("ORR    r0, r0, #(0x3 << 0)");	  // DRL      (4 c)
        asm ("MCR    p15, 1, r0, c9, c0, 2");
        asm ("ISB");
/*
        //---------------------
		//L2 ACTLR 0x118
		//---------------------
       ; MRC p15, 0, r0, c15, c0, 0
	;	ORR r0, r0, #(0x1<<3)
	 ;   ORR r0, r0, #(0x1<<4)
      ;  ORR r0, r0, #(0x1<<8)
	;	AND r0, r0, #0x118
     ;   MCR p15, 1, r0, c15, c0, 0
	;	ISB
		;---------------------
		;L2 PFR/POR 0x9b0
		;---------------------
       ; MRC p15, 1, r0, c15, c0, 3
	;	ORR r0, r0, #0x9b0
	;	AND r0, r0, #0x000009b0
     ;   MCR p15, 1, r0, c15, c0, 3
	  ;  ISB
*/
		//---------------------
		//L1 ACTLR 0x000040
		//--------------------
        asm ("MRC p15, 0, r0, c1, c0, 1");
        asm ("ORR r0, r0, #(0x1<<6)");
        asm ("ORR r0, r0, #(0x3<<27)");   //all writes are allocated
        //   AND r0, r0, #0x000040
        asm ("MCR p15, 0, r0, c1, c0, 1");
        asm ("ISB");
        //POP {r0-r10, lr}

}


void Set_MSMC_As_Sharable(void) {
  unsigned int privid;
  unsigned int sms_reset_val;
  unsigned int ses_reset_val;
  //UWORD32 *log_testpoint;
  // setup logging
  //log_testpoint = (UWORD32 *)(KEPLER_LOGGING_ADDR);
  //*log_testpoint = 0xCC001111;
  // go thru entire MSMC table making sure all unshared bit = 0 which implies all MSMC memory is sharable
  for (privid=0; privid<16; privid++) {
      sms_reset_val = *mkptr(msmc_cfg_base,SMS_MPAXH_0_0+0x40*privid);
     *mkptr(msmc_cfg_base,SMS_MPAXH_0_0+0x40*privid) = (sms_reset_val & 0xFFFFFF7F); // set unshared bit = 0
      ses_reset_val = *mkptr(msmc_cfg_base,SES_MPAXH_0_0+0x40*privid);
     *mkptr(msmc_cfg_base,SES_MPAXH_0_0+0x40*privid) = (ses_reset_val & 0xFFFFFF7F); // set unshared bit = 0
  };
}; // Set_MSMC_As_Sharable



// For the long descriptor format decode, you use the middle (block) diagram of the 64 bit block on page B3-1331 of the ARM ARM(arm reference manual).
// We are using the first level descriptor format.  Page B3-1333 has the lower attributes bits define & page B3-1364 has the SH[1:0] decode.
// -------------------------------------------------------------------------------------------------------------------------------------
// long (64-bit) first-level descriptor on page 1331 of ARM architecture reference manual
// lower page attribute in bits 11:0 (lower 12 bits)
// b11 nG  -> non-global bit = 0
// b10 AF  -> access flag    = 1
// b9:8 SH -> shareability bits:     00 = non-sharable,
//                                   01 = unsupported,
//                                   10 = outer sharble,
//                                   11 = inner sharable
// b7:6 AP -> Access permission = 01
// b5 NS   -> non-secure bit = 1
//
// b4:2  AttrIndex -> attribute index selects 1 of 8 attributes in either MAIR0 or MAIR1 regs
//     NOTE: the MAIR0 & MAIR1 registers have been setup so that the indexes can be used as follows:
//       AttrIndx[4:2]==000 :  Normal memory inner/outer Non-cacheable
//       AttrIndx[4:2]==001 :  Normal memory inner/outer Write back r/w allocate
//       AttrIndx[4:2]==010 :  Device memory
//       AttrIndx[4:2]==011 :  Strongly ordered
//       AttrIndx[4:2]==100 :  Normal memory inner/outer Write-through no allocate
//       AttrIndx[4:2]==101 :  reserved and/or not specified
//       AttrIndx[4:2]==110 :  Normal memory inner/outer write back no allocate
//       AttrIndx[4:2]==111 :  reserved and/or not specified
//
// b1 -> always 0
// b0 -> always 1
// -------------------------------------------------------------------------------------------------------------------------------------

// defines used to setup a 2-LEVEL LPAE table - used for INNER_SHARED false-sharing SRAM testcases
#define NON_SHARED         (0x0<<8)
#define OUTER_SHARED       (0x2<<8)
#define INNER_SHARED       (0x3<<8)
#define NM_IO_NonCached    (0<<2)
#define NM_IO_WB_RWalloc   (1<<2)
#define Device_Memory      (2<<2)
#define Strongly_Ordered   (3<<2)
#define NM_IO_WT_Noalloc   (4<<2)
#define NM_IO_WB_Noalloc   (6<<2)

#define access_perm_rw_pl1         (0x0<<6)
#define access_enable_all_pl_modes (0x1<<6)
#define NS_MODE                    (0x1<<5)
#define mapstoblock                (0x1<<0)
#define mapstotable                (0x3<<0)
#define AF                         (0x1<<10)      // will gen AF if set to 0 and entry fetched to the tlb mcccxliv

// There is 6 MB of MSMC SRAM in Kepler SOC
//  1)  0x0C00_0000 to 0x0C0F_FFFF used for Level-2 LPAE table entries
//  2)  0x0C10_0000 to 0x0C1F_FFFF used for logging tp and semaphores for ARM/GEM communication
//  3)  0x0C20_0000 to 0x0C2F_FFFF used for testing (SRC and DST mem blocks here)
//  4)  0x0C30_0000 to 0x0C3F_FFFF used for 4 x 64bit LPAE table entries
//  5)  0x0C40_0000 to 0x0C4F_FFFF used for ARM compiler text, bss and data
//  6)  0x0C50_0000 to 0x0C5F_FFFF used for ARM compiler code, variables and stacks
/*
void Level_2_lpae_setup(void) {
  // setup a 2-level LPAE in ARM to control SRAM remapping for inner-shared testcases
  unsigned int TABLE_BASE_LPAE;
  unsigned int TABLE_LEV2_LPAE;
  unsigned int index;
  unsigned int lo_32_bits;
  unsigned int hi_32_bits;

  // make sure that all MSMC memory is marked as sharable (default action)
  Set_MSMC_As_Sharable();

  // init table locations
  //TABLE_BASE_LPAE = TABLE_BASE_LPAE_ADDR;  // 0x0C300000 must be unique from LEVEL2 base
  //TABLE_LEV2_LPAE = TABLE_LEV2_LPAE_ADDR;  // 0x0C000000 LEVEL2 base only uses bits 39:21
  TABLE_BASE_LPAE = TTB_LOCATION;  // must be unique from LEVEL2 base
  TABLE_LEV2_LPAE = TTB_LOCATION + 0x1000;  // LEVEL2 base only uses bits 39:21
  hi_32_bits =  0x00000000;
  // setup 4 table entries of 64-bits each
  WR_MEM_64(TABLE_BASE_LPAE+0x00, 0x0000000000000003 | (long long)TABLE_LEV2_LPAE );
  // for SRAM test variations, modify the following LPAE+0x08 table entry
  // NOTE: this is not done yet - we are still using Set_ARM11_Section for most SRAM tests
  WR_MEM_64(TABLE_BASE_LPAE+0x08, 0x0000000040000461);
  // for DDR test variations, modify the LPAE+0x10 table entry for address 0x8000_0000 (DDR3a)
  //                                                      // 12 attr bits = 01 sh 011 idx 01
  //                                                         ------------   -- -- --- --- --

  #if defined (SET_UNSHARABLE_CACHED_WRITEBACK_WRITE_ALLOCATE)
  WR_MEM_64(TABLE_BASE_LPAE+0x10, 0x0000000800000465);    // 12 attr bits = 01_00_011_001_01  <--- cacf0144
#elif defined (SET_SHARABLE_CACHED_WRITEBACK_WRITE_ALLOCATE)
  WR_MEM_64(TABLE_BASE_LPAE+0x10, 0x0000000800000665);    // 12 attr bits = 01_10_011_001_01  <--- cacf0146 - (same as default)
#elif defined (SET_NORMAL_NON_CACHED_BUT_SHARED)
  WR_MEM_64(TABLE_BASE_LPAE+0x10, 0x0000000800000661);    // 12 attr bits = 01_10_011_000_01  <--- cacf0178
#elif defined (SET_SHARED_CACHE_WRITEBACK_NO_ALLOCATE)
  WR_MEM_64(TABLE_BASE_LPAE+0x10, 0x0000000800000679);    // 12 attr bits = 01_10_011_110_01  <--- cacf0180
#elif defined (SET_SHARED_CACHE_WRITE_THROUGH)
  WR_MEM_64(TABLE_BASE_LPAE+0x10, 0x0000000800000671);    // 12 attr bits = 01_10_011_100_01  <--- cacf0182
#elif defined (SET_INNER_SHARED_NORMAL_NON_CACHED)
  WR_MEM_64(TABLE_BASE_LPAE+0x10, 0x0000000800000761);    // 12 attr bits = 01_11_011_000_01  <--- cacf4178 & cacf4378
#elif defined (SET_INNER_SHARED_CACHE_WRITEBACK_NO_ALLOCATE)
  WR_MEM_64(TABLE_BASE_LPAE+0x10, 0x0000000800000779);    // 12 attr bits = 01_11_011_110_01  <--- cacf4180 & cacf4380
#elif defined (SET_INNER_SHARED_CACHE_WRITE_THROUGH)
  WR_MEM_64(TABLE_BASE_LPAE+0x10, 0x0000000800000771);    // 12 attr bits = 01_11_011_100_01  <--- cacf4182 & cacf4382
#elif defined (SET_INNER_SHARED_CACHE_WRITEBACK_WRITE_ALLOCATE)
  WR_MEM_64(TABLE_BASE_LPAE+0x10, 0x0000000800000765);    // 12 attr bits = 01_11_011_001_01  <--- cacf4184 & cacf4384
#else
  WR_MEM_64(TABLE_BASE_LPAE+0x10, 0x0000000800000665);    // 12 attr bits = 01_10_011_001_01  <--- default, Outer sharable, normal memory, write back R/W Allocate
#endif


  WR_MEM_64(TABLE_BASE_LPAE+0x18, 0x0000000900000665);

  // Level 2 translation code: fill in 512 table entries in LEVEL 2 transaction table
  for ( index = 0; index < 512; index++ ) {
     // each entry in Level 2 table configures a 2MB mem block
     if ( index < (0x010>>1) ) {
       // mark internal ROM regions (0x0000_0000 to 0x00FF_FFFF)  as Non-cacheable WB RW_alloc Inner Shared (boot code)
       lo_32_bits = (NM_IO_NonCached | INNER_SHARED) | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
     } else if ( index == (0x0C0>>1) ) {
       // mark MSMC SRAM region  0x0C00_0000 to 0x0C1F_FFFF as non-cacheable Outer Shared (for logging and semaphores)
       lo_32_bits = (NM_IO_NonCached | OUTER_SHARED) | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
    	 //lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
   } else if ( index == (0x0C2>>1) ) {
       // mark MSMC SRAM region  0x0C20_0000 to 0x0C3F_FFFF (could be unique for each testcase)
#if defined (SET_INNER_SHARED_NORMAL_NON_CACHED)
       lo_32_bits = (NM_IO_NonCached  | INNER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ; // <--- cacf4177 & cacf4377
#elif defined (SET_INNER_SHARED_CACHE_WRITEBACK_NO_ALLOCATE)
       lo_32_bits = (NM_IO_WB_Noalloc | INNER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ; // <--- cacf4179 & cacf4379
#elif defined (SET_INNER_SHARED_CACHE_WRITE_THROUGH)
       lo_32_bits = (NM_IO_WT_Noalloc | INNER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ; // <--- cacf4181 & cacf4381
#elif defined (SET_INNER_SHARED_CACHE_WRITEBACK_WRITE_ALLOCATE)
       lo_32_bits = (NM_IO_WB_RWalloc | INNER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ; // <--- cacf4183 & cacf4383
#else
       lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ; // <--- default, should NEVER execute this line!
#endif
     } else if ( index == (0x0C4>>1) ) {
       // mark MSMC SRAM region  0x0C40_0000 to 0x0C5F_FFFF as cacheable WB RW_alloc Outer Shared (for compile code, variables and stacks)
       lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
     } else {
       // Mark remaining memory blocks as strongly ordered non shared (all other memory locations in Kepler SOC)
       lo_32_bits = (Strongly_Ordered | NON_SHARED)   | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
     }
     // take both 32-bit values and write a single 64-bit table entry at the proper position
     WR_MEM_64(TABLE_LEV2_LPAE +(index*8), (((long long)(hi_32_bits)<<32) | (long long)lo_32_bits) );
  }  // for loop
  // tell ARM to use the TTB that was just set up
  //lpae_mmu_enable(TABLE_BASE_LPAE);

}; // Level_2_lpae_setup
*/



void Level_2_lpae_setup(void) {
  // setup a 2-level LPAE in ARM to control SRAM remapping for inner-shared testcases
  unsigned int TTB_LOCATION;
  unsigned int TABLE_LEV2_LPAE_1;
  unsigned int TABLE_LEV2_LPAE_2;
  unsigned int TABLE_LEV2_LPAE_3;
  unsigned int index;
  unsigned int lo_32_bits;
  unsigned int hi_32_bits;
  unsigned int corenum = read_cpu_id();


  // make sure that all MSMC memory is marked as sharable (default action)
  Set_MSMC_As_Sharable();

  // init table locations
  //TABLE_BASE_LPAE = TABLE_BASE_LPAE_ADDR;  // 0x0C300000 must be unique from LEVEL2 base
  //TABLE_LEV2_LPAE = TABLE_LEV2_LPAE_ADDR;  // 0x0C000000 LEVEL2 base only uses bits 39:21
  TTB_LOCATION = 0x90000000;
  TABLE_LEV2_LPAE_1 = TTB_LOCATION + 0x1000;  // must be unique from LEVEL2 base
  TABLE_LEV2_LPAE_2 = TTB_LOCATION + 0x2000;  // LEVEL2 base only uses bits 39:21
  TABLE_LEV2_LPAE_3 = TTB_LOCATION + 0x3000;  // LEVEL2 base only uses bits 39:21
  hi_32_bits =  0x00000000;

  // setup 4 table entries of 64-bits each
  WR_MEM_64(TTB_LOCATION+0x00, 0x0000000000000003 | (long long)TABLE_LEV2_LPAE_1 );
  WR_MEM_64(TTB_LOCATION+0x08, 0x0000000000000003 | (long long)TABLE_LEV2_LPAE_2 );
  WR_MEM_64(TTB_LOCATION+0x10, 0x0000000000000003 | (long long)TABLE_LEV2_LPAE_3 );

  // Level 2 translation code: fill in 512 table entries in LEVEL 2 transaction table
  for ( index = 0; index < 1024; index++ ) {
     // each entry in Level 2 table configures a 2MB mem block
     if ( index < (0x010>>1) ) {
       // mark internal ROM regions (0x0000_0000 to 0x00FF_FFFF)  as Non-cacheable WB RW_alloc Inner Shared (boot code)
       lo_32_bits = (Strongly_Ordered | OUTER_SHARED) | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
     } else if ( index == (0x0C0>>1) ) {
       // mark MSMC SRAM region  0x0C00_0000 to 0x0C1F_FFFF as cacheable Outer Shared
       lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
   } else if ( index == (0x0C2>>1) ) {
       // mark MSMC SRAM region  0x0C20_0000 to 0x0C3F_FFFF (could be unique for each testcase)
#if defined (SET_INNER_SHARED_NORMAL_NON_CACHED)
       lo_32_bits = (NM_IO_NonCached  | INNER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ; // <--- cacf4177 & cacf4377
#elif defined (SET_INNER_SHARED_CACHE_WRITEBACK_NO_ALLOCATE)
       lo_32_bits = (NM_IO_WB_Noalloc | INNER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ; // <--- cacf4179 & cacf4379
#elif defined (SET_INNER_SHARED_CACHE_WRITE_THROUGH)
       lo_32_bits = (NM_IO_WT_Noalloc | INNER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ; // <--- cacf4181 & cacf4381
#elif defined (SET_INNER_SHARED_CACHE_WRITEBACK_WRITE_ALLOCATE)
       lo_32_bits = (NM_IO_WB_RWalloc | INNER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ; // <--- cacf4183 & cacf4383
#else
       //lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ; // <--- default, should NEVER execute this line!
       lo_32_bits = (Strongly_Ordered | OUTER_SHARED) | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
#endif
     } else if ( index == (0x0C4>>1) ) {
       // mark MSMC SRAM region  0x0C40_0000 to 0x0C5F_FFFF as cacheable WB RW_alloc Outer Shared (for compile code, variables and stacks)
       //lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
       lo_32_bits = (Strongly_Ordered | OUTER_SHARED) | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
     } else if ( index <= (0x3FF>>1) ) {
       // Mark remaining memory blocks as strongly ordered non shared (all other memory locations in Kepler SOC)
       lo_32_bits = (Strongly_Ordered | OUTER_SHARED)   | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
     } else {
       // // mark (0x4000_0000 to 0x7FFF_FFFF)  as NM cacheable WB RW_alloc Outer Shared
       //lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED)   | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
    	 lo_32_bits = (NM_IO_NonCached | OUTER_SHARED)   | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
     }
     // take both 32-bit values and write a single 64-bit table entry at the proper position
     WR_MEM_64(TABLE_LEV2_LPAE_1 +(index*8), (((long long)(hi_32_bits)<<32) | (long long)lo_32_bits) );
  }  // for loop


  hi_32_bits =  0x00000008;

  // for DDR test variations, modify the LPAE+0x10 table entry for address 0x8000_0000 (DDR3a)
  //                                                      // 12 attr bits = 01 sh 011 idx 01
  //                                                         ------------   -- -- --- --- --
  //WR_MEM_64(TABLE_BASE_LPAE+0x10, 0x0000000000000003 | (long long)TABLE_LEV2_LPAE );

  // Level 2 translation code: fill in 512 table entries in LEVEL 2 transaction table
   for ( index = 0; index < 512; index++ ) {
      // each entry in Level 2 table configures a 2MB mem block
      if ( index < 32 ) {
        // mark DDR3A region 0x8000_0000 to 0x83FF_FFFF  as cacheable WB RW_alloc Outer Shared
    	lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;

      } else if ( index < 40 ) {
          // mark DDR3A region  0x8400_0000 to 0x87FF_FFFF as private region for each core, cacheable WB RW_alloc Outer Shared
    	  //0x8400_0000 to 0x84FF_FFFF mapped to 0x8400_0000 to 0x84FF_FFFF for Core0
    	  //0x8400_0000 to 0x84FF_FFFF mapped to 0x8500_0000 to 0x85FF_FFFF for Core1
    	  //0x8400_0000 to 0x84FF_FFFF mapped to 0x8600_0000 to 0x86FF_FFFF for Core2
    	  //0x8400_0000 to 0x84FF_FFFF mapped to 0x8700_0000 to 0x87FF_FFFF for Core3
    	  lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF + corenum*(0x1000000) ;

      } else if ( index < 64 ) {
    	  lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF;

      } else if ( index < 96 ) {
        // mark DDR3A region 0x8800_0000 to 0x8BFF_FFFF for non-cacheable, Outer Shared
    	lo_32_bits = (NM_IO_NonCached | OUTER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;

      } else if ( index <104 ) {
        // mark DDR3A region  0x8C00_0000 to 0x8CFF_FFFF as private region for each core, non-cacheable Outer Shared
    	//0x8C00_0000 to 0x8CFF_FFFF mapped to 0x8C00_0000 to 0x8CFF_FFFF for Core0
    	//0x8C00_0000 to 0x8CFF_FFFF mapped to 0x8D00_0000 to 0x8DFF_FFFF for Core1
    	//0x8C00_0000 to 0x8CFF_FFFF mapped to 0x8E00_0000 to 0x8EFF_FFFF for Core2
    	//0x8C00_0000 to 0x8CFF_FFFF mapped to 0x8F00_0000 to 0x8FFF_FFFF for Core3
    	lo_32_bits = (NM_IO_NonCached | OUTER_SHARED) | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF + corenum*(0x1000000) ;

      } else if ( index < 128 ) {
        // Mark remaining memory blocks as cacheable WB RW_alloc Outer Shared
    	lo_32_bits = (NM_IO_NonCached | OUTER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;

      } else {
    	  // Mark remaining memory blocks as normal memory cached outer shared
    	  lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED)   | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
      }
      // take both 32-bit values and write a single 64-bit table entry at the proper position
      WR_MEM_64(TABLE_LEV2_LPAE_3 +(index*8), (((long long)(hi_32_bits)<<32) | (long long)lo_32_bits) );
   }  // for loop


}; // Level_2_lpae_setup




void Level_3_lpae_setup(void) {
  // setup a 3-level LPAE in ARM to control SRAM remapping for inner-shared testcases
  unsigned int TTB_LOCATION;
  unsigned int TABLE_BASE_LPAE;
  unsigned int TABLE_LEV2_LPAE;
  unsigned int TABLE_LEV3_LPAE_MSMC;
  unsigned int TABLE_LEV2_LPAE_DDR;
  unsigned int index, index_1;
  unsigned int lo_32_bits;
  unsigned int hi_32_bits;
  unsigned int corenum = read_cpu_id();


  // make sure that all MSMC memory is marked as sharable (default action)
  Set_MSMC_As_Sharable();

  // init table locations
  //TTB_LOCATION = 0x0C1F0000 + 0x4000*corenum;
  TTB_LOCATION = 0x90000000 + 0x4000*corenum;
  TABLE_BASE_LPAE = TTB_LOCATION;  // must be unique from LEVEL2 base
  TABLE_LEV2_LPAE = TTB_LOCATION + 0x1000;  	// LEVEL2 base only uses bits 39:21
  TABLE_LEV3_LPAE_MSMC = TABLE_LEV2_LPAE + 0x1000;	// LEVEL3 base only uses bits 39:12
  TABLE_LEV2_LPAE_DDR = TABLE_LEV3_LPAE_MSMC + 0x1000;	// LEVEL2 DDR base only uses bits 39:21
  hi_32_bits =  0x00000000;

  // setup 4 table entries of 64-bits each
  WR_MEM_64(TABLE_BASE_LPAE+0x00, 0x0000000000000003 | (long long)TABLE_LEV2_LPAE );
  // for SRAM test variations, modify the following LPAE+0x08 table entry
  // NOTE: this is not done yet - we are still using Set_ARM11_Section for most SRAM tests
  WR_MEM_64(TABLE_BASE_LPAE+0x08, 0x0000000040000461);

  // Level 2 translation code: fill in 512 table entries in LEVEL 2 transaction table
  for ( index = 0; index < 512; index++ ) {
     // each entry in Level 2 table configures a 2MB mem block
     if ( index < (0x010>>1) ) {
       // mark internal ROM regions (0x0000_0000 to 0x00FF_FFFF)  as Non-cacheable WB RW_alloc Inner Shared (boot code)
       lo_32_bits = (NM_IO_NonCached | OUTER_SHARED) | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
     } else if ( index == (0x0C0>>1) ) {
       // MSMC SRAM region  0x0C00_0000 to 0x0C1F_FFFF

    	 for ( index_1 = 0; index_1 < 512; index_1++ ) {
    		 if (index_1 == 0) {
    			 // MSMC SRAM region 0x0C00_0000 to 0x0C00_0FFF, 4KB, cacheable WB RW_alloc Outer Shared (interrupt vector table)
    			 lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index_1<<12) | access_enable_all_pl_modes | NS_MODE | mapstotable | AF;
    		 }
    		 else if (index_1 < 256) {
    			 // MSMC SRAM region 0x0C00_1000 to 0x0C0F_FFFF, 1020KB, cacheable WB RW_alloc Outer Shared (.text)
    			 lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index_1<<12) | access_enable_all_pl_modes | NS_MODE | mapstotable | AF;
    		 }
    		 else if (index_1 < 288){
    			 // MSMC SRAM region 0x0C10_0000 to 0x0C17_FFFF, 128KB*4core=512KB, cacheable WB RW_alloc Outer Shared (private .data for each core)
    			 //0x0C10_0000-0x0C11_FFFF mapped to 0x0C10_0000-0x0C11_FFFF for core0
    			 //0x0C10_0000-0x0C11_FFFF mapped to 0x0C12_0000-0x0C13_FFFF for core1
    			 //0x0C10_0000-0x0C11_FFFF mapped to 0x0C14_0000-0x0C15_FFFF for core2
    			 //0x0C10_0000-0x0C11_FFFF mapped to 0x0C16_0000-0x0C17_FFFF for core3
    			 lo_32_bits = ((NM_IO_WB_RWalloc | OUTER_SHARED) | (index_1<<12)| access_enable_all_pl_modes | NS_MODE | mapstotable | AF) + corenum*(0x20000) ;
    		 }
    	 	 else if ( index_1 < 384) {
    	     	  lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index_1<<12)| access_enable_all_pl_modes | NS_MODE | mapstotable | AF;
    	 	 }
    	 	 else if (index_1 < 464) {
    	 		 // MSMC SRAM region 0x0C18_0000 to 0x0C1E_FFFF, 320KB, non-cacheable, Outer Shared
    	 		lo_32_bits = (NM_IO_NonCached | OUTER_SHARED) | (index_1<<12)| access_enable_all_pl_modes | NS_MODE | mapstotable | AF;
    	 	 }
    	 	 else if (index_1 < 472 ) {
    	 		// MSMC SRAM region 0x0C1D_0000 to 0x0C1D_FFFF, 32KB*4core=128KB non-cacheable,Outer shared
    	 		//0x0C1D_0000-0x0C1D_7FFF mapped to 0x0C1D_0000-0x0C1D_7FFF for core0
    	 		//0x0C1D_0000-0x0C1D_7FFF mapped to 0x0C1D_8000-0x0C1D_FFFF for core1
    	 		//0x0C1D_0000-0x0C1D_7FFF mapped to 0x0C1E_0000-0x0C1E_7FFF for core2
    	 		//0x0C1D_0000-0x0C1D_7FFF mapped to 0x0C1E_8000-0x0C1E_FFFF for core3
    	 		lo_32_bits = ((NM_IO_NonCached | OUTER_SHARED) | (index_1<<12)| access_enable_all_pl_modes | NS_MODE | mapstotable | AF) + corenum*(0x8000) ;
    	 	 }
    	 	 else if (index_1 < 496) {
    	 		lo_32_bits = (NM_IO_NonCached | OUTER_SHARED) | (index_1<<12)| access_enable_all_pl_modes | NS_MODE | mapstotable | AF;
    	 	 }
    	 	 else {
    	 		// MSMC SRAM region 0x0C1F_0000 to 0x0C1F_FFFF, 64KB, cacheable,Outer shared, (MMU table)
    	  		lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index_1<<12)| access_enable_all_pl_modes | NS_MODE | mapstotable | AF;
    	 	 }
    		 // take both 32-bit values and write a single 64-bit table entry at the proper position
    		 WR_MEM_64(TABLE_LEV3_LPAE_MSMC +(index_1*8), (((long long)(hi_32_bits)<<32) | (long long)lo_32_bits | (long long)(0x0c000000)) );
    	 }
    } else if ( index == (0x0C2>>1) ) {
       // mark MSMC SRAM region  0x0C20_0000 to 0x0C3F_FFFF (could be unique for each testcase)
#if defined (SET_INNER_SHARED_NORMAL_NON_CACHED)
       lo_32_bits = (NM_IO_NonCached  | INNER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ; // <--- cacf4177 & cacf4377
#elif defined (SET_INNER_SHARED_CACHE_WRITEBACK_NO_ALLOCATE)
       lo_32_bits = (NM_IO_WB_Noalloc | INNER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ; // <--- cacf4179 & cacf4379
#elif defined (SET_INNER_SHARED_CACHE_WRITE_THROUGH)
       lo_32_bits = (NM_IO_WT_Noalloc | INNER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ; // <--- cacf4181 & cacf4381
#elif defined (SET_INNER_SHARED_CACHE_WRITEBACK_WRITE_ALLOCATE)
       lo_32_bits = (NM_IO_WB_RWalloc | INNER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ; // <--- cacf4183 & cacf4383
#else
       lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ; // <--- default, should NEVER execute this line!
#endif
     } else if ( index == (0x0C4>>1) ) {
       // mark MSMC SRAM region  0x0C40_0000 to 0x0C5F_FFFF as cacheable WB RW_alloc Outer Shared (for compile code, variables and stacks)
       lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
     } else {
       // Mark remaining memory blocks as strongly ordered non shared (all other memory locations in Kepler SOC)
       lo_32_bits = (Strongly_Ordered | OUTER_SHARED)   | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
     }
     // take both 32-bit values and write a single 64-bit table entry at the proper position
     WR_MEM_64(TABLE_LEV2_LPAE +(index*8), (((long long)(hi_32_bits)<<32) | (long long)lo_32_bits) );
  }  // for loop

  // Level 3 translation code: fill in 512 table entries in LEVEL 3 transaction table
  WR_MEM_64(TABLE_LEV2_LPAE+0x300, 0x0000000000000003 | (long long)TABLE_LEV3_LPAE_MSMC );

  // for DDR test variations, modify the LPAE+0x10 table entry for address 0x8000_0000 (DDR3a)
  //                                                      // 12 attr bits = 01 sh 011 idx 01
  //                                                          ------------   -- -- --- --- --
  WR_MEM_64(TABLE_BASE_LPAE+0x10, 0x0000000000000003 | (long long)TABLE_LEV2_LPAE_DDR );
  hi_32_bits =  0x00000008;

  // Level 2 translation code: fill in 512 table entries in LEVEL 2 transaction table
   for ( index = 0; index < 512; index++ ) {
      // each entry in Level 2 table configures a 2MB mem block
      if ( index < 32 ) {
        // mark DDR3A region 0x8000_0000 to 0x83FF_FFFF  as cacheable WB RW_alloc Outer Shared
    	lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;

      } else if ( index < 40 ) {
          // mark DDR3A region  0x8400_0000 to 0x87FF_FFFF as private region for each core, cacheable WB RW_alloc Outer Shared
    	  //0x8400_0000 to 0x84FF_FFFF mapped to 0x8400_0000 to 0x84FF_FFFF for Core0
    	  //0x8400_0000 to 0x84FF_FFFF mapped to 0x8500_0000 to 0x85FF_FFFF for Core1
    	  //0x8400_0000 to 0x84FF_FFFF mapped to 0x8600_0000 to 0x86FF_FFFF for Core2
    	  //0x8400_0000 to 0x84FF_FFFF mapped to 0x8700_0000 to 0x87FF_FFFF for Core3
    	  lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF + corenum*(0x1000000) ;

      } else if ( index < 64 ) {
    	  lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF;

      } else if ( index < 96 ) {
        // mark DDR3A region 0x8800_0000 to 0x8BFF_FFFF for non-cacheable, Outer Shared
    	lo_32_bits = (NM_IO_NonCached | OUTER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;

      } else if ( index <104 ) {
        // mark DDR3A region  0x8C00_0000 to 0x8CFF_FFFF as private region for each core, non-cacheable Outer Shared
    	//0x8C00_0000 to 0x8CFF_FFFF mapped to 0x8C00_0000 to 0x8CFF_FFFF for Core0
    	//0x8C00_0000 to 0x8CFF_FFFF mapped to 0x8D00_0000 to 0x8DFF_FFFF for Core1
    	//0x8C00_0000 to 0x8CFF_FFFF mapped to 0x8E00_0000 to 0x8EFF_FFFF for Core2
    	//0x8C00_0000 to 0x8CFF_FFFF mapped to 0x8F00_0000 to 0x8FFF_FFFF for Core3
    	lo_32_bits = (NM_IO_NonCached | OUTER_SHARED) | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF + corenum*(0x1000000) ;

      } else if ( index < 128 ) {
        // Mark remaining memory blocks as cacheable WB RW_alloc Outer Shared
    	lo_32_bits = (NM_IO_NonCached | OUTER_SHARED) | (index<<21)| access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;

      } else {
    	  // Mark remaining memory blocks as strongly ordered non shared (all other memory locations in Kepler SOC)
    	  lo_32_bits = (NM_IO_WB_RWalloc | OUTER_SHARED)   | (index<<21) | access_enable_all_pl_modes | NS_MODE | mapstoblock | AF ;
      }
      // take both 32-bit values and write a single 64-bit table entry at the proper position
      WR_MEM_64(TABLE_LEV2_LPAE_DDR +(index*8), (((long long)(hi_32_bits)<<32) | (long long)lo_32_bits) );
   }  // for loop

   //map DDR3A region 0xc000_0000~0xFFFF_FFFF to 0x9_0000_0000~0x9_3FFF_FFFF, cacheable WB RW_alloc Outer Shared
   WR_MEM_64(TABLE_BASE_LPAE+0x18, 0x0000000900000665);

   //Enable LPAE MMU
   lpae_mmu_enable(TTB_LOCATION);

}; // Level_3_lpae_setup


void DSB_inst (void)
{
	asm ("DSB");
}
#if 0
void data_copy (void)
{
	asm ("ldr     r0,=__data_load_start") ;
	asm ("ldr     r1,=__data_start ");
	asm ("ldr     r2,=__data_end");
	asm ("1: ");
	asm ("cmp     r1,r2 ");
	asm ("ldmltia r0!,{r3}" );
	asm ("stmltia r1!,{r3} ");
	asm ("blt     1b");
}
#endif
