diff options
Diffstat (limited to 'ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.S')
| -rw-r--r-- | ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.S | 91 |
1 files changed, 85 insertions, 6 deletions
diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.S b/ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.S index 90d2c4b92f..2cde8e2039 100644 --- a/ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.S +++ b/ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.S @@ -33,43 +33,58 @@ .globl ASM_PFX(ArmDisableExtendPTConfig) .globl ASM_PFX(ArmEnableBranchPrediction) .globl ASM_PFX(ArmDisableBranchPrediction) +.globl ASM_PFX(ArmV7AllDataCachesOperation) .set DC_ON, (0x1<<2) .set IC_ON, (0x1<<12) -.set XP_ON, (0x1<<23) + ASM_PFX(ArmInvalidateDataCacheEntryByMVA): - mcr p15, 0, r0, c7, c6, 1 @invalidate single data cache line + mcr p15, 0, r0, c7, c6, 1 @invalidate single data cache line + dsb + isb bx lr ASM_PFX(ArmCleanDataCacheEntryByMVA): mcr p15, 0, r0, c7, c10, 1 @clean single data cache line + dsb + isb bx lr ASM_PFX(ArmCleanInvalidateDataCacheEntryByMVA): mcr p15, 0, r0, c7, c14, 1 @clean and invalidate single data cache line + dsb + isb bx lr ASM_PFX(ArmInvalidateDataCacheEntryBySetWay): mcr p15, 0, r0, c7, c6, 2 @ Invalidate this line + dsb + isb bx lr ASM_PFX(ArmCleanInvalidateDataCacheEntryBySetWay): mcr p15, 0, r0, c7, c14, 2 @ Clean and Invalidate this line + dsb + isb bx lr ASM_PFX(ArmCleanDataCacheEntryBySetWay): mcr p15, 0, r0, c7, c10, 2 @ Clean this line + dsb + isb bx lr ASM_PFX(ArmDrainWriteBuffer): mcr p15, 0, r0, c7, c10, 4 @ Drain write buffer for sync + dsb + isb bx lr @@ -77,7 +92,8 @@ ASM_PFX(ArmInvalidateInstructionCache): mov R0,#0 mcr p15,0,R0,c7,c5,0 @Invalidate entire instruction cache mov R0,#0 - mcr p15,0,R0,c7,c5,4 @Instruction synchronization barrier + dsb + isb bx LR ASM_PFX(ArmEnableMmu): @@ -99,9 +115,8 @@ ASM_PFX(ArmDisableMmu): bic R0,R0,#1 mcr p15,0,R0,c1,c0,0 @Disable MMU mov R0,#0 - mcr p15,0,R0,c7,c10,4 @Data synchronization barrier - mov R0,#0 - mcr p15,0,R0,c7,c5,4 @Instruction synchronization barrier + dsb + isb bx LR ASM_PFX(ArmEnableDataCache): @@ -109,6 +124,8 @@ ASM_PFX(ArmEnableDataCache): mrc p15,0,R0,c1,c0,0 @Read control register configuration data orr R0,R0,R1 @Set C bit mcr p15,0,r0,c1,c0,0 @Write control register configuration data + dsb + isb bx LR ASM_PFX(ArmDisableDataCache): @@ -116,6 +133,8 @@ ASM_PFX(ArmDisableDataCache): mrc p15,0,R0,c1,c0,0 @Read control register configuration data bic R0,R0,R1 @Clear C bit mcr p15,0,r0,c1,c0,0 @Write control register configuration data + dsb + isb bx LR ASM_PFX(ArmEnableInstructionCache): @@ -123,6 +142,8 @@ ASM_PFX(ArmEnableInstructionCache): mrc p15,0,R0,c1,c0,0 @Read control register configuration data orr R0,R0,R1 @Set I bit mcr p15,0,r0,c1,c0,0 @Write control register configuration data + dsb + isb bx LR ASM_PFX(ArmDisableInstructionCache): @@ -130,18 +151,76 @@ ASM_PFX(ArmDisableInstructionCache): mrc p15,0,R0,c1,c0,0 @Read control register configuration data bic R0,R0,R1 @Clear I bit. mcr p15,0,r0,c1,c0,0 @Write control register configuration data + dsb + isb bx LR ASM_PFX(ArmEnableBranchPrediction): mrc p15, 0, r0, c1, c0, 0 orr r0, r0, #0x00000800 mcr p15, 0, r0, c1, c0, 0 + dsb + isb bx LR ASM_PFX(ArmDisableBranchPrediction): mrc p15, 0, r0, c1, c0, 0 bic r0, r0, #0x00000800 mcr p15, 0, r0, c1, c0, 0 + dsb + isb bx LR + +ASM_PFX(ArmV7AllDataCachesOperation): + stmfd SP!,{r4-r12, LR} + mov R1, R0 @ Save Function call in R1 + mrc p15, 1, R6, c0, c0, 1 @ Read CLIDR + ands R3, R6, #0x7000000 @ Mask out all but Level of Coherency (LoC) + mov R3, R3, LSR #23 @ Cache level value (naturally aligned) + beq L_Finished + mov R10, #0 + +Loop1: + add R2, R10, R10, LSR #1 @ Work out 3xcachelevel + mov R12, R6, LSR R2 @ bottom 3 bits are the Cache type for this level + and R12, R12, #7 @ get those 3 bits alone + cmp R12, #2 + blt L_Skip @ no cache or only instruction cache at this level + mcr p15, 2, R10, c0, c0, 0 @ write the Cache Size selection register (CSSELR) // OR in 1 for Instruction + isb @ ISB to sync the change to the CacheSizeID reg + mcr p15, 1, R12, c0, c0, 0 @ reads current Cache Size ID register (CCSIDR) + and R2, R12, #0x7 @ extract the line length field + and R2, R2, #4 @ add 4 for the line length offset (log2 16 bytes) + mov R4, #0x400 + sub R4, R4, #1 + ands R4, R4, R12, LSR #3 @ R4 is the max number on the way size (right aligned) + clz R5, R4 @ R5 is the bit position of the way size increment + mov R7, #0x00008000 + sub R7, R7, #1 + ands R7, R7, R12, LSR #13 @ R7 is the max number of the index size (right aligned) + +Loop2: + mov R9, R4 @ R9 working copy of the max way size (right aligned) + +Loop3: + orr R0, R10, R9, LSL R5 @ factor in the way number and cache number into R11 + orr R0, R0, R7, LSL R2 @ factor in the index number + + blx R1 + + subs R9, R9, #1 @ decrement the way number + bge Loop3 + subs R7, R7, #1 @ decrement the index + bge Loop2 +L_Skip: + add R10, R10, #2 @ increment the cache number + cmp R3, R10 + bgt Loop1 + +L_Finished: + ldmfd SP!, {r4-r12, lr} + bx LR + + ASM_FUNCTION_REMOVE_IF_UNREFERENCED |