/**********************************************************/
/* Sample program : Debug ARM Assembly Program            */
/* File Name      : d_armasm.s                            */
/* Copyright (C) Renesas Electronics Corp. 2015.          */
/**********************************************************/

	.INCLUDE		"boot_mon.h"
	.ALIGN	4


;# uint32_t MonCp15Mpidr(void);
STARTFUNC MonCp15Mpidr
	MRS		X0, VMPIDR_EL2		;# read Multiprocessor ID register
	RET
ENDFUNC MonCp15Mpidr


;# uint32_t MonCp15Midr(void);
STARTFUNC MonCp15Midr
	MRS		X0, VPIDR_EL2		;# read Processor ID Register
	RET
ENDFUNC MonCp15Midr


;# void WriteTCR_EL3( uint64_t data );
;#   data[63:39]=RES0
;#   data[38][37][36][35][34:32]=TBI1,TBI0,AS,RES0,IPS
;#   data[31:30][29:28][27:26][25:24][23][22][21:16]=TG1,SH1,ORGN1,IRGN1,EPD1,A1  ,T1SZ
;#   data[15:14][13:12][11:10][09:08][07][06][05:00]=TG0,SH0,ORGN0,IRGN0,EPD0,RES0,T0SZ
STARTFUNC WriteTCR_EL3
	MSR TCR_EL3, X0
	RET
ENDFUNC WriteTCR_EL3


;# void WriteMAIR_EL3( uint64_t data );
;#   data[63:56][55:48][47:40][39:32]=Attr7,Attr6,Attr5,Attr4
;#   data[31:24][23:16][15: 8][ 7: 0]=Attr3,Attr2,Attr1,Attr0
STARTFUNC WriteMAIR_EL3
	MSR MAIR_EL3, X0
	RET
ENDFUNC WriteMAIR_EL3


;# void WriteTTBR0_EL3( uint64_t data );
;#   data[63:48]=ASID
;#   data[47: 0]=BADDR
STARTFUNC WriteTTBR0_EL3
	MSR TTBR0_EL3, X0
	RET
ENDFUNC WriteTTBR0_EL3


;# void CleaningAndInvalidateICache(void);
STARTFUNC CleaningAndInvalidateICache
	IC IALLUIS
	ISB SY
	RET
ENDFUNC CleaningAndInvalidateICache


;# Refer: DEN0024A_v8_architecture_PG.pdf
;# void CleaningDCache(void);
STARTFUNC CleaningDCache
	MRS X0, CLIDR_EL1
	AND W3, W0, #0x07000000		// Get 2 x Level of Coherence
	LSR W3, W3, #23
	CBZ W3, DC_Finished
	MOV W10, #0					// W10 = 2 x cache level
	MOV W8, #1					// W8 = constant 0b1
DC_Loop1:
	ADD W2, W10, W10, LSR #1	// Calculate 3 x cache level
	LSR W1, W0, W2				// extract 3-bit cache type for this level
	AND W1, W1, #0x7
	CMP W1, #2
	B.LT DC_Skip					// No data or unified cache at this level
	MSR CSSELR_EL1, X10			// Select this cache level
	ISB							// Synchronize change of CSSELR
	MRS X1, CCSIDR_EL1			// Read CCSIDR
	AND W2, W1, #7				// W2 = log2(linelen)-4
	ADD W2, W2, #4				// W2 = log2(linelen)
	UBFX W4, W1, #3, #10		// W4 = max way number, right aligned
	CLZ W5, W4					// W5 = 32-log2(ways), bit position of way in DC operand
	LSL W9, W4, W5				// W9 = max way number, aligned to position in DC operand
	LSL W16, W8, W5				// W16 = amount to decrement way number per iteration
DC_Loop2:
	UBFX W7, W1, #13, #15		// W7 = max set number, right aligned
	LSL W7, W7, W2				// W7 = max set number, aligned to position in DC operand
	LSL W17, W8, W2				// W17 = amount to decrement set number per iteration
DC_Loop3:
	ORR W11, W10, W9			// W11 = combine way number and cache number...
	ORR W11, W11, W7			// ... and set number for DC operand
	DC CSW, X11					// Do data cache clean by set and way
	SUBS W7, W7, W17			// Decrement set number
	B.GE DC_Loop3
	SUBS X9, X9, X16			// Decrement way number
	B.GE DC_Loop2
DC_Skip:
	ADD W10, W10, #2			// Increment 2 x cache level
	CMP W3, W10
	DSB SY						// Ensure completion of previous cache maintenance operation
	B.GT DC_Loop1
DC_Finished:
	DSB SY						;# Add DSB
	ISB							;# Add ISB
	RET
ENDFUNC CleaningDCache


;# Refer: DEN0024A_v8_architecture_PG.pdf
;# uint32_t CleaningAndInvalidateDCache(void);
STARTFUNC CleaningAndInvalidateDCache
	MRS X0, CLIDR_EL1
	AND W3, W0, #0x07000000		// Get 2 x Level of Coherence
	LSR W3, W3, #23
	CBZ W3, DCI_Finished
	MOV W10, #0					// W10 = 2 x cache level
	MOV W8, #1					// W8 = constant 0b1
DCI_Loop1:
	ADD W2, W10, W10, LSR #1	// Calculate 3 x cache level
	LSR W1, W0, W2				// extract 3-bit cache type for this level
	AND W1, W1, #0x7
	CMP W1, #2
	B.LT DCI_Skip				// No data or unified cache at this level
	MSR CSSELR_EL1, X10			// Select this cache level
	ISB							// Synchronize change of CSSELR
	MRS X1, CCSIDR_EL1			// Read CCSIDR
	AND W2, W1, #7				// W2 = log2(linelen)-4
	ADD W2, W2, #4				// W2 = log2(linelen)
	UBFX W4, W1, #3, #10		// W4 = max way number, right aligned
	CLZ W5, W4					// W5 = 32-log2(ways), bit position of way in DC operand
	LSL W9, W4, W5				// W9 = max way number, aligned to position in DC operand
	LSL W16, W8, W5				// W16 = amount to decrement way number per iteration
DCI_Loop2:
	UBFX W7, W1, #13, #15		// W7 = max set number, right aligned
	LSL W7, W7, W2				// W7 = max set number, aligned to position in DC operand
	LSL W17, W8, W2				// W17 = amount to decrement set number per iteration
DCI_Loop3:
	ORR W11, W10, W9			// W11 = combine way number and cache number...
	ORR W11, W11, W7			// ... and set number for DC operand
	DC CISW, X11				// Do data cache clean and invalidate by set and way
	SUBS W7, W7, W17			// Decrement set number
	B.GE DCI_Loop3
	SUBS X9, X9, X16			// Decrement way number
	B.GE DCI_Loop2
DCI_Skip:
	ADD W10, W10, #2			// Increment 2 x cache level
	CMP W3, W10
	DSB SY						// Ensure completion of previous cache maintenance operation
	B.GT DCI_Loop1
DCI_Finished:
	DSB SY						;# Add DSB
	ISB							;# Add ISB
	RET
ENDFUNC CleaningAndInvalidateDCache


;# void DCacheEnable(void);
STARTFUNC DCacheEnable
	MOV		X20, X30			;# Save LR data to X20

	BL		SetVmsaTable		;# MMU Table setting

	ISB							;# The ISB forces these changes to be seen before the MMU is enabled.
	MRS		X0, SCTLR_EL3		;# Read System Control Register configuration data
	ORR		X0, X0, #1			;# Set [M] bit and enable the MMU.
	ORR		X0, X0, #4			;# Set [C] bit and enable the Data Cache.
	DSB SY
	MSR		SCTLR_EL3, X0		;# Write System Control Register configuration data
	ISB							;# The ISB forces these changes to be seen by the next instruction

	MOV		X30, X20			;# Load LR data from X20
	RET
ENDFUNC DCacheEnable


;# void DCacheDisable(void);
STARTFUNC DCacheDisable
	MOV		X20, X30			;# Save LR data to X20

	DSB SY
	ISB							;# 
	MRS		X0, SCTLR_EL3		;# 
;#	ORR		X0, X0, #1			;# Set [M] bit and enable the MMU.
;#	ORR		X0, X0, #4			;# Set [C] bit and enable the Data Cache.
	bic		X0, X0, #1			;# Set [M] bit and disable the MMU.
	bic		X0, X0, #4			;# Set [C] bit and disable the Data Cache.
	DSB SY
	MSR		SCTLR_EL3, X0		;# 
	DSB SY
	ISB							;# 

	BL		CleaningDCache
	BL		CleaningAndInvalidateDCache
	DSB SY
	ISB							;# 

	MOV		X30, X20			;# Load LR data from X20
	RET
ENDFUNC DCacheDisable


STARTFUNC InterruptDisableDAIF
	MRS		X0, DAIF			;# Read DAIF
	LDR		W2, =0xC0			;# bit[7]:IRQ mask bit, bit[6]:FIQ mask bit
	ORR		X0,X0,X2			;# => 0:Exception not masked, 1: Exception masked
	MSR		DAIF, X0			;# Write DAIF
	RET
ENDFUNC InterruptDisableDAIF


STARTFUNC ReadSCR_EL3
	MRS		X0, SCR_EL3			;# Read SCR_EL3
	RET
ENDFUNC ReadSCR_EL3


STARTFUNC InterruptDisableSCR_EL3
	MRS		X0, SCR_EL3			;# Read SCR_EL3
	LDR		W2, =0x6			;# bit[2]:Physical FIQ Routing, bit[1]:Physical IRQ Routing
	BIC		X0,X0,X2			;# => 0:Interrupt not taken, 1: Interrupt are taken
	MSR		SCR_EL3, X0			;# Write SCR_EL3
	RET
ENDFUNC InterruptDisableSCR_EL3


STARTFUNC DropToEl1
	MRS		X0, HCR_EL2			;# Read HCR_EL2
	LDR		W2, =0x80000000		;# bit[31]:EL1 is AArch64
	ORR		X0,X0,X2			;# => 0:Lower levels are all AArch32 1:EL1 is AArch64
	MSR		HCR_EL2, X0			;# Write HCR_EL2
	
	MRS		X0, SCR_EL3			;# Read SCR_EL3
	LDR		W2, =0x400			;# bit[10]:EL1 is AArch64
	ORR		X0,X0,X2			;# => 0:Lower levels are all AArch32 1:EL2/1 are AArch64
	MSR		SCR_EL3, X0			;# Write SCR_EL3
	
	LDR		W0, =0x3C5			;# bit[3:0]:EL1h
	MSR		SPSR_EL3, X0		;# Write SPSR_EL3
	
	MOV		X0, SP
	MSR		SP_EL1,X0
	MOV		X0, X30
	MSR		ELR_EL3,X0
	ERET
ENDFUNC DropToEl1

STARTFUNC UpToEl3
	LDR		W0, =0x3CD			;# bit[3:0]:EL3h
	MSR		SPSR_EL3, X0		;# Write SPSR_EL3
	
	MRS		X0, SP_EL1			;# Read SP_EL1
	MOV		SP, X0
;#	MOV		X0, X30
;#	MRS		X0, ELR_EL1			;# Read ELR_EL1
;#	MSR		ELR_EL3,X0
	RET
ENDFUNC UpToEl3


STARTFUNC ChangeNonSecure
	MRS		X0, SCR_EL3			;# Read SCR_EL3
	LDR		W2, =0x1			;# bit[0]:Non-secure bit
;#	LDR		W2, =0x481			;# bit[0]:Non-secure bit
	ORR		X0,X0,X2			;# => 0:Secure, 1:Non-secure
	MSR		SCR_EL3, X0			;# Write SCR_EL3
	RET
ENDFUNC ChangeNonSecure


STARTFUNC ChangeSecure
	MRS		X0, SCR_EL3			;# Read SCR_EL3
	LDR		W2, =0x0			;# bit[0]:Non-secure bit
	ORR		X0,X0,X2			;# => 0:Secure, 1:Non-secure
	MSR		SCR_EL3, X0			;# Write SCR_EL3
	RET
ENDFUNC ChangeSecure


;# void SoftDelayAsm(uint32_t count);  キャッシュONの場合、2clockで1周可能 (AArch64)
STARTFUNC SoftDelayAsm			;# 引数をそのまま減算に使用する
	SUBS	W0, W0, #1			;# フラグを更新しつつ減算	1step
	BNE		SoftDelayAsm		;# R0==0になるまでループ	1step
	RET							;# 呼び出し元に戻る
ENDFUNC SoftDelayAsm


	.END
