289 lines
8.4 KiB
ArmAsm
289 lines
8.4 KiB
ArmAsm
/**********************************************************/
|
||
/* 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); <20>L<EFBFBD><4C><EFBFBD>b<EFBFBD>V<EFBFBD><56>ON<4F>̏ꍇ<CC8F>A2clock<63><6B>1<EFBFBD><31><EFBFBD>\ (AArch64)
|
||
STARTFUNC SoftDelayAsm ;# <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̂܂܌<DC82><DC8C>Z<EFBFBD>Ɏg<C98E>p<EFBFBD><70><EFBFBD><EFBFBD>
|
||
SUBS W0, W0, #1 ;# <20>t<EFBFBD><74><EFBFBD>O<EFBFBD><4F><EFBFBD>X<EFBFBD>V<EFBFBD><56><EFBFBD><C282><C28C>Z 1step
|
||
BNE SoftDelayAsm ;# R0==0<>ɂȂ<C982><C882>܂Ń<DC82><C583>[<5B>v 1step
|
||
RET ;# <20>Ăяo<D18F><6F><EFBFBD><EFBFBD><EFBFBD>ɖ߂<C996>
|
||
ENDFUNC SoftDelayAsm
|
||
|
||
|
||
.END
|