sravan/system76-edk2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Sync gcc with armasm. update some memory barriers.

Browse Source 
git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@10025 6f19259b-4bc3-4df7-8a09-765794883524
This commit is contained in:
andrewfish
2010-02-19 18:51:10 +00:00
parent 752d258a42
commit 98bc0c8c05
10 changed files with 570 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
ArmPkg/Library/ArmLib/ArmV7/ArmLibSupport.S
View File

@@ -25,6 +25,9 @@
.globl ASM_PFX(ArmSetDomainAccessControl)
.globl ASM_PFX(CPSRMaskInsert)
.globl ASM_PFX(CPSRRead)
.globl ASM_PFX(ReadCCSIDR)
.globl ASM_PFX(ReadCLIDR)
#------------------------------------------------------------------------------
@@ -37,18 +40,11 @@ ASM_PFX(Cp15CacheInfo):
bx LR
ASM_PFX(ArmEnableInterrupts):
mrs R0,CPSR
bic R0,R0,#0x80 @Enable IRQ interrupts
msr CPSR_c,R0
cpsie i
bx LR
ASM_PFX(ArmDisableInterrupts):
mrs R0,CPSR
orr R1,R0,#0x80 @Disable IRQ interrupts
msr CPSR_c,R1
tst R0,#0x80
moveq R0,#1
movne R0,#0
cpsid i
bx LR
ASM_PFX(ArmGetInterruptState):
@@ -61,10 +57,12 @@ ASM_PFX(ArmGetInterruptState):
ASM_PFX(ArmInvalidateTlb):
mov r0,#0
mcr p15,0,r0,c8,c7,0
isb
bx lr
ASM_PFX(ArmSetTranslationTableBaseAddress):
mcr p15,0,r0,c2,c0,0
isb
bx lr
ASM_PFX(ArmGetTranslationTableBaseAddress):
@@ -74,6 +72,7 @@ ASM_PFX(ArmGetTranslationTableBaseAddress):
ASM_PFX(ArmSetDomainAccessControl):
mcr p15,0,r0,c3,c0,0
isb
bx lr
ASM_PFX(CPSRMaskInsert): @ on entry, r0 is the mask and r1 is the field to insert
@@ -92,4 +91,14 @@ ASM_PFX(CPSRRead):
mrs r0, cpsr
bx lr
ASM_PFX(ReadCCSIDR):
mcr p15,2,r0,c0,c0,0 @ Write Cache Size Selection Register (CSSELR)
isb
mrc p15,1,r0,c0,c0,0 @ Read current CP15 Cache Size ID Register (CCSIDR)
bx lr
ASM_PFX(ReadCLIDR):
mrc p15,1,r0,c0,c0,1 @ Read CP15 Cache Level ID Register
ASM_FUNCTION_REMOVE_IF_UNREFERENCED

71
ArmPkg/Library/ArmLib/ArmV7/ArmLibSupport.asm
View File

@@ -24,95 +24,55 @@
EXPORT ArmSetDomainAccessControl
EXPORT CPSRMaskInsert
EXPORT CPSRRead
EXPORT ReadCCSIDR
AREA ArmLibSupport, CODE, READONLY
Cp15IdCode
DSB
ISB
mrc p15,0,R0,c0,c0,0
DSB
ISB
bx LR
Cp15CacheInfo
DSB
ISB
mrc p15,0,R0,c0,c0,1
DSB
ISB
bx LR
ArmEnableInterrupts
DSB
ISB
mrs R0,CPSR
bic R0,R0,#0x80 ;Enable IRQ interrupts
msr CPSR_c,R0
DSB
ISB
CPSIE i
bx LR
ArmDisableInterrupts
DSB
ISB
mrs R0,CPSR
orr R1,R0,#0x80 ;Disable IRQ interrupts
msr CPSR_c,R1
tst R0,#0x80
moveq R0,#1
movne R0,#0
DSB
ISB
CPSID i
bx LR
ArmGetInterruptState
DSB
ISB
mrs R0,CPSR
tst R0,#0x80 ;Check if IRQ is enabled.
moveq R0,#1
movne R0,#0
DSB
ISB
bx LR
ArmInvalidateTlb
DSB
ISB
mov r0,#0
mcr p15,0,r0,c8,c7,0
DSB
ISB
bx lr
ArmSetTranslationTableBaseAddress
DSB
ISB
mcr p15,0,r0,c2,c0,0
DSB
ISB
bx lr
ArmGetTranslationTableBaseAddress
DSB
ISB
mrc p15,0,r0,c2,c0,0
DSB
ISB
bx lr
ArmSetDomainAccessControl
DSB
ISB
mcr p15,0,r0,c3,c0,0
DSB
ISB
bx lr
CPSRMaskInsert ; on entry, r0 is the mask and r1 is the field to insert
DSB
ISB
stmfd sp!, {r4-r12, lr} ; save all the banked registers
mov r3, sp ; copy the stack pointer into a non-banked register
mrs r2, cpsr ; read the cpsr
@@ -120,20 +80,33 @@ CPSRMaskInsert ; on entry, r0 is the mask and r1 is the field to in
and r1, r1, r0 ; clear bits outside the mask in the input
orr r2, r2, r1 ; set field
msr cpsr_cxsf, r2 ; write back cpsr (may have caused a mode switch)
ISB
mov sp, r3 ; restore stack pointer
ldmfd sp!, {r4-r12, lr} ; restore registers
DSB
ISB
bx lr ; return (hopefully thumb-safe!)
CPSRRead
DSB
ISB
mrs r0, cpsr
DSB
ISB
bx lr
// UINT32
// ReadCCSIDR (
// IN UINT32 CSSELR
// )
ReadCCSIDR
MCR p15,2,r0,c0,c0,0 ; Write Cache Size Selection Register (CSSELR)
ISB
MRC p15,1,<Rt>,c0,c0,0 ; Read current CP15 Cache Size ID Register (CCSIDR)
BX lr
// UINT32
// ReadCLIDR (
// IN UINT32 CSSELR
// )
ReadCLIDR
MRC p15,1,<Rt>,c0,c0,1 ; Read CP15 Cache Level ID Register
END

130
ArmPkg/Library/ArmLib/ArmV7/ArmV7Lib.c
View File

@@ -11,13 +11,14 @@
WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
**/
#include <Uefi.h>
#include <Chipset/ArmV7.h>
#include <Library/ArmLib.h>
#include <Library/BaseLib.h>
#include <Library/BaseMemoryLib.h>
#include <Library/MemoryAllocationLib.h>
#include "ArmV7Lib.h"
#include "ArmLibPrivate.h"
VOID
FillTranslationTable (
@@ -136,7 +137,9 @@ ArmCacheArchitecture (
VOID
)
{
return ARM_CACHE_ARCHITECTURE_SEPARATE;
UINT32 CLIDR = ReadCLIDR ();
return CLIDR; // BugBug Fix Me
}
BOOLEAN
@@ -145,7 +148,18 @@ ArmDataCachePresent (
VOID
)
{
return TRUE;
UINT32 CLIDR = ReadCLIDR ();
if ((CLIDR & 0x2) == 0x2) {
// Instruction cache exists
return TRUE;
}
if ((CLIDR & 0x7) == 0x4) {
// Unified cache
return TRUE;
}
return FALSE;
}
UINTN
@@ -154,7 +168,17 @@ ArmDataCacheSize (
VOID
)
{
return 16 * 1024;
UINT32 NumSets;
UINT32 Associativity;
UINT32 LineSize;
UINT32 CCSIDR = ReadCCSIDR (0);
LineSize = (1 << (CCSIDR + 2));
Associativity = ((CCSIDR >> 3) & 0x3ff) + 1;
NumSets = ((CCSIDR >> 13) & 0x7fff) + 1;
// LineSize is in words (4 byte chunks)
return NumSets * Associativity * LineSize * 4;
}
UINTN
@@ -163,7 +187,9 @@ ArmDataCacheAssociativity (
VOID
)
{
return 4;
UINT32 CCSIDR = ReadCCSIDR (0);
return ((CCSIDR >> 3) & 0x3ff) + 1;
}
UINTN
@@ -171,7 +197,9 @@ ArmDataCacheSets (
VOID
)
{
return 64;
UINT32 CCSIDR = ReadCCSIDR (0);
return ((CCSIDR >> 13) & 0x7fff) + 1;
}
UINTN
@@ -180,7 +208,10 @@ ArmDataCacheLineLength (
VOID
)
{
return 64;
UINT32 CCSIDR = ReadCCSIDR (0) & 7;
// * 4 converts to bytes
return (1 << (CCSIDR + 2)) * 4;
}
BOOLEAN
@@ -189,7 +220,18 @@ ArmInstructionCachePresent (
VOID
)
{
return TRUE;
UINT32 CLIDR = ReadCLIDR ();
if ((CLIDR & 1) == 1) {
// Instruction cache exists
return TRUE;
}
if ((CLIDR & 0x7) == 0x4) {
// Unified cache
return TRUE;
}
return FALSE;
}
UINTN
@@ -198,7 +240,17 @@ ArmInstructionCacheSize (
VOID
)
{
return 16 * 1024;
UINT32 NumSets;
UINT32 Associativity;
UINT32 LineSize;
UINT32 CCSIDR = ReadCCSIDR (1);
LineSize = (1 << (CCSIDR + 2));
Associativity = ((CCSIDR >> 3) & 0x3ff) + 1;
NumSets = ((CCSIDR >> 13) & 0x7fff) + 1;
// LineSize is in words (4 byte chunks)
return NumSets * Associativity * LineSize * 4;
}
UINTN
@@ -207,55 +259,53 @@ ArmInstructionCacheAssociativity (
VOID
)
{
return 4;
UINT32 CCSIDR = ReadCCSIDR (1);
return ((CCSIDR >> 3) & 0x3ff) + 1;
// return 4;
}
UINTN
EFIAPI
ArmInstructionCacheSets (
VOID
)
{
UINT32 CCSIDR = ReadCCSIDR (1);
return ((CCSIDR >> 13) & 0x7fff) + 1;
}
UINTN
EFIAPI
ArmInstructionCacheLineLength (
VOID
)
{
return 64;
UINT32 CCSIDR = ReadCCSIDR (1) & 7;
// * 4 converts to bytes
return (1 << (CCSIDR + 2)) * 4;
// return 64;
}
VOID
ArmV7DataCacheOperation (
IN ARM_V7_CACHE_OPERATION DataCacheOperation
)
{
UINTN Set;
UINTN SetCount;
UINTN SetShift;
UINTN Way;
UINTN WayCount;
UINTN WayShift;
UINT32 SetWayFormat;
UINTN SavedInterruptState;
SetCount = ArmDataCacheSets();
WayCount = ArmDataCacheAssociativity();
SavedInterruptState = ArmGetInterruptState ();
// ARMv7 Manual, System Control Coprocessor chapter
SetShift = 6;
WayShift = 32 - LowBitSet32 ((UINT32)WayCount);
ArmV7AllDataCachesOperation (DataCacheOperation);
SavedInterruptState = ArmDisableInterrupts();
for (Way = 0; Way < WayCount; Way++) {
for (Set = 0; Set < SetCount; Set++) {
// Build the format that the CP15 instruction can understand
SetWayFormat = (Way << WayShift) | (Set << SetShift);
// Pass it through
(*DataCacheOperation)(SetWayFormat);
}
}
ArmDrainWriteBuffer();
ArmDrainWriteBuffer ();
if (SavedInterruptState) {
ArmEnableInterrupts();
ArmEnableInterrupts ();
}
}
@@ -265,7 +315,7 @@ ArmInvalidateDataCache (
VOID
)
{
ArmV7DataCacheOperation(ArmInvalidateDataCacheEntryBySetWay);
ArmV7DataCacheOperation (ArmInvalidateDataCacheEntryBySetWay);
}
VOID
@@ -274,7 +324,7 @@ ArmCleanInvalidateDataCache (
VOID
)
{
ArmV7DataCacheOperation(ArmCleanInvalidateDataCacheEntryBySetWay);
ArmV7DataCacheOperation (ArmCleanInvalidateDataCacheEntryBySetWay);
}
VOID
@@ -283,5 +333,5 @@ ArmCleanDataCache (
VOID
)
{
ArmV7DataCacheOperation(ArmCleanDataCacheEntryBySetWay);
ArmV7DataCacheOperation (ArmCleanDataCacheEntryBySetWay);
}

1
ArmPkg/Library/ArmLib/ArmV7/ArmV7Lib.h
View File

@@ -15,7 +15,6 @@
#ifndef __ARM_V7_LIB_H__
#define __ARM_V7_LIB_H__
typedef VOID (*ARM_V7_CACHE_OPERATION)(UINT32);
VOID
EFIAPI

91
ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.S
View File

@@ -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

96
ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.asm
View File

@@ -32,7 +32,6 @@
DC_ON EQU ( 0x1:SHL:2 )
IC_ON EQU ( 0x1:SHL:12 )
XP_ON EQU ( 0x1:SHL:23 )
AREA ArmCacheLib, CODE, READONLY
@@ -40,17 +39,13 @@ XP_ON EQU ( 0x1:SHL:23 )
ArmInvalidateDataCacheEntryByMVA
DSB
ISB
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
ArmCleanDataCacheEntryByMVA
DSB
ISB
MCR p15, 0, r0, c7, c10, 1 ; clean single data cache line
DSB
ISB
@@ -58,8 +53,6 @@ ArmCleanDataCacheEntryByMVA
ArmCleanInvalidateDataCacheEntryByMVA
DSB
ISB
MCR p15, 0, r0, c7, c14, 1 ; clean and invalidate single data cache line
DSB
ISB
@@ -67,8 +60,6 @@ ArmCleanInvalidateDataCacheEntryByMVA
ArmInvalidateDataCacheEntryBySetWay
DSB
ISB
mcr p15, 0, r0, c7, c6, 2 ; Invalidate this line
DSB
ISB
@@ -76,8 +67,6 @@ ArmInvalidateDataCacheEntryBySetWay
ArmCleanInvalidateDataCacheEntryBySetWay
DSB
ISB
mcr p15, 0, r0, c7, c14, 2 ; Clean and Invalidate this line
DSB
ISB
@@ -85,8 +74,6 @@ ArmCleanInvalidateDataCacheEntryBySetWay
ArmCleanDataCacheEntryBySetWay
DSB
ISB
mcr p15, 0, r0, c7, c10, 2 ; Clean this line
DSB
ISB
@@ -94,8 +81,6 @@ ArmCleanDataCacheEntryBySetWay
ArmDrainWriteBuffer
DSB
ISB
mcr p15, 0, r0, c7, c10, 4 ; Drain write buffer for sync
DSB
ISB
@@ -103,8 +88,6 @@ ArmDrainWriteBuffer
ArmInvalidateInstructionCache
DSB
ISB
MOV R0,#0
MCR p15,0,R0,c7,c5,0 ;Invalidate entire instruction cache
MOV R0,#0
@@ -114,8 +97,6 @@ ArmInvalidateInstructionCache
BX LR
ArmEnableMmu
DSB
ISB
mrc p15,0,R0,c1,c0,0
orr R0,R0,#1
mcr p15,0,R0,c1,c0,0
@@ -124,33 +105,22 @@ ArmEnableMmu
bx LR
ArmMmuEnabled
DSB
ISB
mrc p15,0,R0,c1,c0,0
and R0,R0,#1
DSB
ISB
bx LR
ArmDisableMmu
DSB
ISB
mov R0,#0
mcr p15,0,R0,c13,c0,0 ;FCSE PID register must be cleared before disabling MMU
mrc p15,0,R0,c1,c0,0
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
ArmEnableDataCache
DSB
ISB
LDR R1,=DC_ON
MRC p15,0,R0,c1,c0,0 ;Read control register configuration data
ORR R0,R0,R1 ;Set C bit
@@ -160,56 +130,90 @@ ArmEnableDataCache
BX LR
ArmDisableDataCache
DSB
ISB
LDR R1,=DC_ON
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
ArmEnableInstructionCache
DSB
ISB
LDR R1,=IC_ON
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
ArmDisableInstructionCache
DSB
ISB
LDR R1,=IC_ON
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
ArmEnableBranchPrediction
DSB
ISB
mrc p15, 0, r0, c1, c0, 0
orr r0, r0, #0x00000800
mcr p15, 0, r0, c1, c0, 0
DSB
ISB
bx LR
ArmDisableBranchPrediction
DSB
ISB
mrc p15, 0, r0, c1, c0, 0
bic r0, r0, #0x00000800
mcr p15, 0, r0, c1, c0, 0
DSB
ISB
bx LR
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, #&7000000 ; Mask out all but Level of Coherency (LoC)
MOV R3, R3, LSR #23 ; Cache level value (naturally aligned)
BEQ 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 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
MRC p15, 1, R12, c0, c0, 0 ; reads current Cache Size ID register (CCSIDR)
AND R2, R12, #&7 ; extract the line length field
ADD R2, R2, #4 ; add 4 for the line length offset (log2 16 bytes)
LDR R4, =0x3FF
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
LDR R7, =0x00007FFF
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
Skip
ADD R10, R10, #2 ; increment the cache number
CMP R3, R10
BGT Loop1
Finished
LDMFD SP!, {r4-r12, lr}
BX LR
END