/**********************************************************/
/* Sample program : VMSA Table Generate                   */
/* File Name      : vmsatable.c                           */
/* Copyright (C) Renesas Electronics Corp. 2015.          */
/**********************************************************/

// #include "common.h"
#include <stdint.h>
#include "vmsatable.h"
#include "d_armasm.h"

#ifdef AArch64
extern const char MMU_BASE_CPU0[];
#endif
#ifdef AArch32
#define MMU_BASE_CPU0	0xE6300000
#endif

#define TBL_SIZE	4096
#define TBL_NUM		(TBL_SIZE/8)

#define Lvl1VmsaTbl		((uintptr_t)MMU_BASE_CPU0+0x0000)		// +0kB
#define Lvl2VmsaTbl		((uintptr_t)MMU_BASE_CPU0+0x1000)		// +4kB
#define Lvl3VmsaTbl		((uintptr_t)MMU_BASE_CPU0+0x2000)		// +8kB

#define Lvl1StartAddr	(0x0000000000)
#define Lvl1BlockSize	(0x0040000000)  // 1GB
#define Lvl1BlockShift	(30)            // BIT30
#define Lvl2StartAddr	(0x00C0000000)
#define Lvl2BlockSize	(0x0000200000)  // 2MB
#define Lvl2BlockShift	(21)            // BIT21
#define Lvl3StartAddr	(0x00E6200000)
#define Lvl3BlockSize	(0x0000001000)  // 4KB
#define Lvl3BlockShift	(12)            // BIT12

#define BIT47_12	0x0000FFFFFFFFF000

typedef struct vmsaTable{
	uint64_t vAddr;
	uint64_t pAddr;
	uint64_t unit;
	uint64_t upperAttr;
	uint64_t lowerAttr;
}vmsaTable;


//------------------------------------------------------------------
// Referenced: DDI0487A_f_armv8_arm.pdf
//             D4.4 VMSAv8-64 translation table format descriptors
//------------------------------------------------------------------

#define ATTR_TBL		0x444444FF0C080400	// MAIR_EL1
//                                          // [63:56]Attr7=0x44 : -----------------     (Blank)
//                                          // [55:48]Attr6=0x44 : -----------------     (Blank)
//                                          // [47:40]Attr5=0x44 : NORMAL_NON_CACHEABLE  Normal Memory Non-Cacheable
//                                          // [39:32]Attr4=0x77 : NORMAL_WRITE_BACK     Normal Memory Write-back transient
//                                          // [31:24]Attr3=0x0C : DEVICE_GRE_MEM        Device-GRE memory
//                                          // [23:16]Attr2=0x08 : DEVICE_NGRE_MEM       Device-nGRE memory
//                                          // [15: 8]Attr1=0x04 : DEVICE_NGNRE_MEM      Device-nGnRE memory
//                                          // [ 7: 0]Attr0=0x00 : DEVICE_NGNRNE_MEM     Device-nGnRnE memory

// CPU0 VMSA table Level1 (4kB)
// Virtual address == Physical Address
const vmsaTable ArmVmsaTblLvl1Cpu0ForDDR4ch[] = {
																				// [MEM]Memory Address           ...BIT[1:0]=01
																				// [TBL]Next Level Table Address ...BIT[1:0]=11
	// Virtual Address   Physical Address    BLOCK   Upper       Lower                                                    Attr
	// (Input)                  or           units,  Attributes  Attributes                           nG  AF  SH   AP  NS Indx [1:0]
	//                   Next Level Table   (1GBxN)  [63:52]     [11:00]                                    [1:0][2:1]   [2:0]
	{    0x0000000000,     0x0000000000,      1,     0x000,      0x411 },	// [MEM]CS0/1,PCIe  1GB  0   1   00   00  0  100   01  (Normal Memory Write-back transient)
	{    0x0040000000,     0x0040000000,      1,     0x000,      0x411 },	// [MEM]DRAM 1st    1GB  0   1   00   00  0  100   01  (Normal Memory Write-back transient)
	{    0x0080000000,     0x0080000000,      1,     0x000,      0x411 },	// [MEM]DRAM 2nd    1GB  0   1   00   00  0  100   01  (Normal Memory Write-back transient)
	{    0x00C0000000,     Lvl2VmsaTbl ,      1,     0x000,      0x003 },	// [TBL]IPs         1GB  0   0   00   00  0  000   11  (Next Level Table)
	{    0x0400000000,     0x0400000000,      4,     0x000,      0x411 },	// [MEM]DDR0        4GB  0   1   10   00  0  100   01  (Normal Memory Write-back transient)
	{    0x0500000000,     0x0500000000,      4,     0x000,      0x411 },	// [MEM]DDR1        4GB  0   1   10   00  0  100   01  (Normal Memory Write-back transient)
	{    0x0600000000,     0x0600000000,      4,     0x000,      0x411 },	// [MEM]DDR2        4GB  0   1   10   00  0  100   01  (Normal Memory Write-back transient)
	{    0x0700000000,     0x0700000000,      4,     0x000,      0x411 },	// [MEM]DDR3        4GB  0   1   10   00  0  100   01  (Normal Memory Write-back transient)
	{    0x0000000000,     0x0000000000,      0,     0x000,      0x000 },	// ---------- END of Table ----------
};

// CPU0 VMSA table Level1 (4kB)
// Virtual address == Physical Address
const vmsaTable ArmVmsaTblLvl1Cpu0ForDDR2ch[] = {
																				// [MEM]Memory Address           ...BIT[1:0]=01
																				// [TBL]Next Level Table Address ...BIT[1:0]=11
	// Virtual Address   Physical Address    BLOCK   Upper       Lower                                                    Attr
	// (Input)                  or           units,  Attributes  Attributes                           nG  AF  SH   AP  NS Indx [1:0]
	//                   Next Level Table   (1GBxN)  [63:52]     [11:00]                                    [1:0][2:1]   [2:0]
	{    0x0000000000,     0x0000000000,      1,     0x000,      0x411 },	// [MEM]CS0/1,PCIe  1GB  0   1   00   00  0  100   01  (Normal Memory Write-back transient)
	{    0x0040000000,     0x0040000000,      1,     0x000,      0x411 },	// [MEM]DRAM 1st    1GB  0   1   00   00  0  100   01  (Normal Memory Write-back transient)
	{    0x0080000000,     0x0080000000,      1,     0x000,      0x411 },	// [MEM]DRAM 2nd    1GB  0   1   00   00  0  100   01  (Normal Memory Write-back transient)
	{    0x00C0000000,     Lvl2VmsaTbl ,      1,     0x000,      0x003 },	// [TBL]IPs         1GB  0   0   00   00  0  000   11  (Next Level Table)
	{    0x0400000000,     0x0400000000,      4,     0x000,      0x411 },	// [MEM]DDR0        4GB  0   1   10   00  0  100   01  (Normal Memory Write-back transient)
	{    0x0600000000,     0x0600000000,      4,     0x000,      0x411 },	// [MEM]DDR2        4GB  0   1   10   00  0  100   01  (Normal Memory Write-back transient)
	{    0x0000000000,     0x0000000000,      0,     0x000,      0x000 },	// ---------- END of Table ----------
};

// CPU0 VMSA table Level1 (4kB)
// Virtual address == Physical Address
const vmsaTable ArmVmsaTblLvl1Cpu0ForDDR1ch[] = {
																				// [MEM]Memory Address           ...BIT[1:0]=01
																				// [TBL]Next Level Table Address ...BIT[1:0]=11
	// Virtual Address   Physical Address    BLOCK   Upper       Lower                                                    Attr
	// (Input)                  or           units,  Attributes  Attributes                           nG  AF  SH   AP  NS Indx [1:0]
	//                   Next Level Table   (1GBxN)  [63:52]     [11:00]                                    [1:0][2:1]   [2:0]
	{    0x0000000000,     0x0000000000,      1,     0x000,      0x411 },	// [MEM]CS0/1,PCIe  1GB  0   1   00   00  0  100   01  (Normal Memory Write-back transient)
	{    0x0040000000,     0x0040000000,      1,     0x000,      0x411 },	// [MEM]DRAM 1st    1GB  0   1   00   00  0  100   01  (Normal Memory Write-back transient)
	{    0x0080000000,     0x0080000000,      1,     0x000,      0x411 },	// [MEM]DRAM 2nd    1GB  0   1   00   00  0  100   01  (Normal Memory Write-back transient)
	{    0x00C0000000,     Lvl2VmsaTbl ,      1,     0x000,      0x003 },	// [TBL]IPs         1GB  0   0   00   00  0  000   11  (Next Level Table)
	{    0x0400000000,     0x0400000000,      4,     0x000,      0x411 },	// [MEM]DDR0        4GB  0   1   10   00  0  100   01  (Normal Memory Write-back transient)
	{    0x0000000000,     0x0000000000,      0,     0x000,      0x000 },	// ---------- END of Table ----------
};

// CPU0 VMSA table Level2 (4kB)
// Virtual address == Physical Address
const vmsaTable ArmVmsaTblLvl2Cpu0[] = {
																				// [MEM]Memory Address           ...BIT[1:0]=01
																				// [TBL]Next Level Table Address ...BIT[1:0]=11
	// Virtual Address   Physical Address    BLOCK   Upper       Lower                                                    Attr
	// (Input)                  or           units,  Attributes  Attributes                           nG  AF  SH   AP  NS Indx [1:0]
	//                   Next Level Table   (2MBxN)  [63:52]     [11:00]                                    [1:0][2:1]   [2:0]
	{    0x00E6200000,     Lvl3VmsaTbl ,      1,     0x000,      0x003 },	// [TBL]IPs          2MB  0   0   00   00  0  000   11  (Next Level Table)
	{    0x00EB200000,     0x00EB200000,      1,     0x000,      0x411 },	// [MEM]RT-SRAM      2MB  0   1   00   00  0  100   01  (Normal Memory Write-back transient) @V3U
	{    0x0000000000,     0x0000000000,      0,     0x000,      0x000 },	// ---------- END of Table ----------
};

#ifdef SYSTEM_DEF_FALCON
// CPU0 VMSA table Level3 (4kB) @V3U CPUボード到着後に上のvmsaTable ArmVmsaTblLvl3Cpu0[]と入れ替え
// Virtual address == Physical Address
const vmsaTable ArmVmsaTblLvl3Cpu0[] = {
																				// [INV]Invalid         ...BIT[1:0]=x0
																				// [RES]Reserved        ...BIT[1:0]=01
																				// [MEM]Memory Address  ...BIT[1:0]=11 <-- Select
	// Virtual Address   Physical Address    BLOCK   Upper       Lower                                                    Attr
	// (Input)                  or           units,  Attributes  Attributes                           nG  AF  SH   AP  NS Indx [1:0]
	//                   Next Level Table   (4KBxN)  [63:52]     [11:00]                                    [1:0][2:1]   [2:0]
	{    0x00E6200000,     0x00E6200000,    256,     0x000,      0x403 },	// [MEM]IPs          1MB  0   1   00   00  0  000   11  (Device-nGnRnE memory)
	{    0x00E6300000,     0x00E6300000,    256,     0x000,      0x413 },	// [MEM]SystemRAM    1MB  0   1   00   00  0  100   11  (Normal Memory Write-back transient) @V3U
	{    0x0000000000,     0x0000000000,      0,     0x000,      0x000 },	// ---------- END of Table ----------
};

#else
// CPU0 VMSA table Level3 (4kB) @V3H
// Virtual address == Physical Address
const vmsaTable ArmVmsaTblLvl3Cpu0[] = {
																				// [INV]Invalid         ...BIT[1:0]=x0
																				// [RES]Reserved        ...BIT[1:0]=01
																				// [MEM]Memory Address  ...BIT[1:0]=11 <-- Select
	// Virtual Address   Physical Address    BLOCK   Upper       Lower                                                    Attr
	// (Input)                  or           units,  Attributes  Attributes                           nG  AF  SH   AP  NS Indx [1:0]
	//                   Next Level Table   (4KBxN)  [63:52]     [11:00]                                    [1:0][2:1]   [2:0]
	{    0x00E6200000,     0x00E6200000,    256,     0x000,      0x403 },	// [MEM]IPs          1MB  0   1   00   00  0  000   11  (Device-nGnRnE memory)
	{    0x00E6300000,     0x00E6300000,     48,     0x000,      0x413 },	// [MEM]SystemRAM  192KB  0   1   00   00  0  100   11  (Normal Memory Write-back transient)
	{    0x00E6330000,     0x00E6330000,     48,     0x000,      0x413 },	// [MEM]SystemRAM  192KB  0   1   00   00  0  100   11  (Normal Memory Write-back transient)
	{    0x00E6360000,     0x00E6360000,    160,     0x000,      0x403 },	// [MEM]SystemRAM  640KB  0   1   00   00  0  000   11  (Device-nGnRnE memory)
	{    0x0000000000,     0x0000000000,      0,     0x000,      0x000 },	// ---------- END of Table ----------
};
#endif

static void MakeVmsaTable(uint64_t *vmsaTblSadd, vmsaTable *vmsaSrcTbl, uint64_t startAddr, uint64_t blockShift);

uint32_t SetVmsaTable(void)
{
	uint64_t *vmsaTblSadd;
	vmsaTable *vmsaSrcTbl;
	uint64_t startAddr;
	uint64_t blockShift;

	// TCR_EL3, Translation Control Register (EL3)
	// [31]    Reserved-1: 1
	// [30:29] Reserved-0: 0
	//
	// When ARMv8.2-TTPBHA is implemented
	// [28]    HWU62     : 0      :Bit[62] of each stage 1 translation table Block or Page entry cannot be used by hardware for an IMPLEMENTATION DEFINED purpose.
	// [27]    HWU61     : 0      :Bit[61] of each stage 1 translation table Block or Page entry cannot be used by hardware for an IMPLEMENTATION DEFINED purpose.
	// [26]    HWU60     : 0      :Bit[60] of each stage 1 translation table Block or Page entry cannot be used by hardware for an IMPLEMENTATION DEFINED purpose.
	// [25]    HWU59     : 0      :Bit[59] of each stage 1 translation table Block or Page entry cannot be used by hardware for an IMPLEMENTATION DEFINED purpose.
	//                             -The Effective value of this field is 0 if the value of TCR_EL3.HPD is 0.
	//                             -This field resets to an architecturally UNKNOWN value.
	// [24]    HPD       : 0      :Hierarchical Permission Disables.
	// Otherwise
	// [28:24] Reserved-0: 00000
	//
	// [23]    Reserved-1: 1
	// When ARMv8.1-TTHM is implemented
	// [22]    HD        : 0      :Hardware management of dirty state in stage 1 translations from EL3.
	// [21]    HA        : 0      :Hardware Access flag update in stage 1 translations from EL3.
	// Otherwise
	// [22:21] Reserved-0: 00
	//
	// [20]    TBI       : 0      : Top Byte used in the address calculation.
	// [19]    Reserved-0: 0
	// [18:16] PS        : 001    : Physical Address Size 000=32bit, 001=36bit, 010=40bit, 011=42bit, 100=44bit, 101=48bit
	// [15:14] TG0       : 00     : translation table 00=4KB, 01=64KB, 10=16KB
	// [13:12] SH0       : 10     : translation table memory 00=Non-shareable, 10=Outer Shareable, 11=Inner Shareable
	// [11:10] ORGN0     : 01     : Outer 01=Write-Back Write-Allocate Cacheable
	// [09:08] IRGN0     : 01     : Inner 01=Write-Back Write-Allocate Cacheable
	// [07:06] Reserved  : 00
	// [05:00] T0SZ      : 011100 : The region size is 2^(64-T0SZ) byte.  2^(64-28)=2^36=0x10_0000_0000
	WriteTCR_EL3(0x8081251C);
	WriteMAIR_EL3(ATTR_TBL);
	WriteTTBR0_EL3((uint64_t)Lvl1VmsaTbl);			// ASID=0
#ifdef SYSTEM_DEF_FALCON
	// VMSA Table Level1 for Main
	vmsaSrcTbl  = (vmsaTable*)ArmVmsaTblLvl1Cpu0ForDDR4ch;
#else
	// VMSA Table Level1 for Main
//	if( CHK_H3 && (!CHK_H3N) ){ vmsaSrcTbl  = (vmsaTable*)ArmVmsaTblLvl1Cpu0ForDDR4ch; }	// 4ch : H3
//	else if( CHK_M3          ){ vmsaSrcTbl  = (vmsaTable*)ArmVmsaTblLvl1Cpu0ForDDR2ch; }	// 2ch : M3
//	else if( CHK_H3N         ){ vmsaSrcTbl  = (vmsaTable*)ArmVmsaTblLvl1Cpu0ForDDR2ch; }	// 2ch : H3N
//	else                      { vmsaSrcTbl  = (vmsaTable*)ArmVmsaTblLvl1Cpu0ForDDR1ch; }	// 1ch : M3N,V3H,V3M,D3,E3
#endif
	vmsaSrcTbl  = (vmsaTable*)ArmVmsaTblLvl1Cpu0ForDDR4ch;
	
	vmsaTblSadd = (uint64_t*)Lvl1VmsaTbl;
	startAddr   = Lvl1StartAddr;
	blockShift  = Lvl1BlockShift;
	MakeVmsaTable(vmsaTblSadd, vmsaSrcTbl, startAddr, blockShift);

	// VMSA Table Level2 for IPs
	vmsaSrcTbl  = (vmsaTable*)ArmVmsaTblLvl2Cpu0;
	vmsaTblSadd = (uint64_t*)Lvl2VmsaTbl;
	startAddr   = Lvl2StartAddr;
	blockShift  = Lvl2BlockShift;
	MakeVmsaTable(vmsaTblSadd, vmsaSrcTbl, startAddr, blockShift);

	// VMSA Table Level3 for IPs
	vmsaSrcTbl  = (vmsaTable*)ArmVmsaTblLvl3Cpu0;
	vmsaTblSadd = (uint64_t*)Lvl3VmsaTbl;
	startAddr   = Lvl3StartAddr;
	blockShift  = Lvl3BlockShift;
	MakeVmsaTable(vmsaTblSadd, vmsaSrcTbl, startAddr, blockShift);

	return 0;
}

static void MakeVmsaTable(uint64_t *vmsaTblSadd, vmsaTable *vmsaSrcTbl, uint64_t startAddr, uint64_t blockShift)
{
	uint32_t i,j;
	uint64_t setData;
	uint64_t *setAddr;
	uint64_t upperAttr;		// Attribute fields for VMSAv8-64 ([63:52])
	uint64_t lowerAttr;		// Attribute fields for VMSAv8-64 ([11:0])
	uint64_t virAdd;
	uint64_t phyAdd;
	uint64_t block;
	uint64_t blockSize;

	blockSize = (1<<blockShift);

	// Level1 : make default table (non-cacheable)
	upperAttr = 0x000; // [63:52] : [52]=Contiguous=1? (T.B.D.)
	lowerAttr = 0x401; // [11:0]  : nG=0, AF=1, SH=00, AP=00, NS=0, AttrIndx=000, [1:0]=01
	setAddr   = vmsaTblSadd;
	setData   = (upperAttr<<52)|(startAddr&BIT47_12)|(lowerAttr);
	for(i=0;i<TBL_NUM;i++){
		*setAddr = setData;
		setAddr++;				// Virtual Address (+1GB)(+2MB)
		setData += blockSize;	// Physical Address (+1GB)(+2MB)
	}

	// Level1 :
	// Descriptor Address[47:12] = TTBR[47:12]
	// Descriptor Address[11:03] = Input Address[38:30]
	// Descriptor Address[02:00] = 0,0,0
	// ----------
	// Descriptor[47:12] = Physical Address[47:12] or Level2 table address[47:12]
	for(i=0; vmsaSrcTbl[i].unit!=0 ;i++){
		virAdd    = vmsaSrcTbl[i].vAddr;
		phyAdd    = vmsaSrcTbl[i].pAddr;
		setAddr   = (uint64_t*)(uintptr_t)((uintptr_t)vmsaTblSadd + ((virAdd-startAddr)>>(blockShift-3)));
		setData   = vmsaSrcTbl[i].upperAttr<<52;			// [63:52]
		setData  |= phyAdd;									// [47:12]
		setData  |= vmsaSrcTbl[i].lowerAttr;				// [11:00]

		block = vmsaSrcTbl[i].unit;
		for(j=0; j<block; j++){
			*setAddr = setData;
			setAddr++;				// Virtual Address (+1GB)(+2MB)
			setData += blockSize;	// Physical Address (+1GB)(+2MB)
		}
	}
}

