[mirror_edk2.git] / ArmPkg / Library / ArmLib / ArmV7 / ArmV7Support.asm

//------------------------------------------------------------------------------ 
//
// Copyright (c) 2008 - 2009, Apple Inc. All rights reserved.<BR>
//
// This program and the accompanying materials
// are licensed and made available under the terms and conditions of the BSD License
// which accompanies this distribution.  The full text of the license may be found at
// http://opensource.org/licenses/bsd-license.php
//
// THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
// WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
//
//------------------------------------------------------------------------------

    EXPORT  ArmInvalidateInstructionCache
    EXPORT  ArmInvalidateDataCacheEntryByMVA
    EXPORT  ArmCleanDataCacheEntryByMVA
    EXPORT  ArmCleanInvalidateDataCacheEntryByMVA
    EXPORT  ArmInvalidateDataCacheEntryBySetWay
    EXPORT  ArmCleanDataCacheEntryBySetWay
    EXPORT  ArmCleanInvalidateDataCacheEntryBySetWay
    EXPORT  ArmDrainWriteBuffer
    EXPORT  ArmEnableMmu
    EXPORT  ArmDisableMmu
    EXPORT  ArmDisableCachesAndMmu
    EXPORT  ArmMmuEnabled
    EXPORT  ArmEnableDataCache
    EXPORT  ArmDisableDataCache
    EXPORT  ArmEnableInstructionCache
    EXPORT  ArmDisableInstructionCache
    EXPORT  ArmEnableSWPInstruction
    EXPORT  ArmEnableBranchPrediction
    EXPORT  ArmDisableBranchPrediction
    EXPORT  ArmV7AllDataCachesOperation
    EXPORT  ArmDataMemoryBarrier
    EXPORT  ArmDataSyncronizationBarrier
    EXPORT  ArmInstructionSynchronizationBarrier
    EXPORT  ArmWriteNsacr
    EXPORT  ArmWriteScr
    EXPORT  ArmWriteVMBar
    EXPORT  ArmWriteVBar
    EXPORT  ArmReadVBar
    EXPORT  ArmWriteCPACR
    EXPORT  ArmEnableVFP
    EXPORT  ArmCallWFI
    EXPORT  ArmWriteAuxCr
    EXPORT  ArmReadAuxCr
    EXPORT  ArmReadCbar
    EXPORT  ArmInvalidateInstructionAndDataTlb
    EXPORT  ArmReadMpidr

    AREA    ArmCacheLib, CODE, READONLY
    PRESERVE8

DC_ON           EQU     ( 0x1:SHL:2 )
IC_ON           EQU     ( 0x1:SHL:12 )
CTRL_M_BIT      EQU     (1 << 0)
CTRL_C_BIT      EQU     (1 << 2)
CTRL_B_BIT      EQU     (1 << 7)
CTRL_I_BIT      EQU     (1 << 12)


ArmInvalidateDataCacheEntryByMVA
  mcr     p15, 0, r0, c7, c6, 1   ; invalidate single data cache line       
  dsb
  isb
  bx      lr


ArmCleanDataCacheEntryByMVA
  mcr     p15, 0, r0, c7, c10, 1  ; clean single data cache line     
  dsb
  isb
  bx      lr


ArmCleanInvalidateDataCacheEntryByMVA
  mcr     p15, 0, r0, c7, c14, 1  ; clean and invalidate single data cache line
  dsb
  isb
  bx      lr


ArmInvalidateDataCacheEntryBySetWay
  mcr     p15, 0, r0, c7, c6, 2        ; Invalidate this line    
  dsb
  isb
  bx      lr


ArmCleanInvalidateDataCacheEntryBySetWay
  mcr     p15, 0, r0, c7, c14, 2       ; Clean and Invalidate this line    
  dsb
  isb
  bx      lr


ArmCleanDataCacheEntryBySetWay
  mcr     p15, 0, r0, c7, c10, 2       ; Clean this line    
  dsb
  isb
  bx      lr


ArmInvalidateInstructionCache
  mcr     p15,0,R0,c7,c5,0      ;Invalidate entire instruction cache
  isb
  bx      LR

ArmEnableMmu
  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)
  orr     R0,R0,#1              ; Set SCTLR.M bit : Enable MMU
  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)
  dsb
  isb
  bx      LR

ArmMmuEnabled
  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)
  and     R0,R0,#1
  bx      LR

ArmDisableMmu
  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)
  bic     R0,R0,#1              ; Clear SCTLR.M bit : Disable MMU
  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)

  mcr     p15,0,R0,c8,c7,0      ; TLBIALL : Invalidate unified TLB
  mcr     p15,0,R0,c7,c5,6      ; BPIALL  : Invalidate entire branch predictor array
  dsb
  isb
  bx      LR

ArmDisableCachesAndMmu
  mrc   p15, 0, r0, c1, c0, 0           ; Get control register
  bic   r0, r0, #CTRL_M_BIT             ; Disable MMU
  bic   r0, r0, #CTRL_C_BIT             ; Disable D Cache
  bic   r0, r0, #CTRL_I_BIT             ; Disable I Cache
  mcr   p15, 0, r0, c1, c0, 0           ; Write control register
  dsb
  isb
  bx      LR

ArmEnableDataCache
  ldr     R1,=DC_ON             ; Specify SCTLR.C bit : (Data) Cache enable bit
  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)
  orr     R0,R0,R1              ; Set SCTLR.C bit : Data and unified caches enabled
  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)
  dsb
  isb
  bx      LR
    
ArmDisableDataCache
  ldr     R1,=DC_ON             ; Specify SCTLR.C bit : (Data) Cache enable bit
  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)
  bic     R0,R0,R1              ; Clear SCTLR.C bit : Data and unified caches disabled
  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)
  isb
  bx      LR

ArmEnableInstructionCache
  ldr     R1,=IC_ON             ; Specify SCTLR.I bit : Instruction cache enable bit
  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)
  orr     R0,R0,R1              ; Set SCTLR.I bit : Instruction caches enabled
  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)
  dsb
  isb
  bx      LR
  
ArmDisableInstructionCache
  ldr     R1,=IC_ON             ; Specify SCTLR.I bit : Instruction cache enable bit
  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)
  BIC     R0,R0,R1              ; Clear SCTLR.I bit : Instruction caches disabled
  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)
  isb
  bx      LR

ArmEnableSWPInstruction
  mrc     p15, 0, r0, c1, c0, 0
  orr     r0, r0, #0x00000400
  mcr     p15, 0, r0, c1, c0, 0
  isb
  bx      LR

ArmEnableBranchPrediction
  mrc     p15, 0, r0, c1, c0, 0 ; Read SCTLR into R0 (Read control register configuration data)
  orr     r0, r0, #0x00000800   ;
  mcr     p15, 0, r0, c1, c0, 0 ; Write R0 into SCTLR (Write control register configuration data)
  isb
  bx      LR

ArmDisableBranchPrediction
  mrc     p15, 0, r0, c1, c0, 0 ; Read SCTLR into R0 (Read control register configuration data)
  bic     r0, r0, #0x00000800   ;
  mcr     p15, 0, r0, c1, c0, 0 ; Write R0 into SCTLR (Write control register configuration data)
  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
  dsb
  ldmfd SP!, {r4-r12, lr}
  bx    LR


ArmDataMemoryBarrier
  dmb
  bx      LR
  
ArmDataSyncronizationBarrier
ArmDrainWriteBuffer
  dsb
  bx      LR
  
ArmInstructionSynchronizationBarrier
  isb
  bx      LR

ArmWriteNsacr
  mcr     p15, 0, r0, c1, c1, 2
  bx      lr

ArmWriteScr
  mcr     p15, 0, r0, c1, c1, 0
  bx      lr

ArmWriteAuxCr
  mcr     p15, 0, r0, c1, c0, 1
  bx      lr

ArmReadAuxCr
  mrc     p15, 0, r0, c1, c0, 1
  bx      lr  

ArmWriteVMBar
  mcr     p15, 0, r0, c12, c0, 1
  bx      lr

ArmWriteVBar
  mcr     p15, 0, r0, c12, c0, 0 
  bx      lr

ArmReadVBar
  mrc     p15, 0, r0, c12, c0, 0 
  bx      lr

ArmWriteCPACR
  mcr     p15, 0, r0, c1, c0, 2
  bx      lr

ArmEnableVFP
  // Enable VFP registers
  mrc     p15, 0, r0, c1, c0, 2
  orr     r0, r0, #0x00f00000   // Enable VPF access (V* instructions)
  mcr     p15, 0, r0, c1, c0, 2
  mov     r0, #0x40000000       // Set EN bit in FPEXC
  mcr     p10,#0x7,r0,c8,c0,#0  // msr     FPEXC,r0 in ARM assembly
  bx      lr

ArmCallWFI
  wfi
  bx      lr

//Note: Return 0 in Uniprocessor implementation
ArmReadCbar
  mrc     p15, 4, r0, c15, c0, 0  //Read Configuration Base Address Register
  bx      lr

ArmInvalidateInstructionAndDataTlb
  mcr     p15, 0, r0, c8, c7, 0      ; Invalidate Inst TLB and Data TLB
  dsb
  bx lr

ArmReadMpidr
  mrc     p15, 0, r0, c0, c0, 5    ; read MPIDR
  bx      lr

  END
Commit	Line	Data
2ef2b01e A	1	//------------------------------------------------------------------------------
2ef2b01e A	2	//
d6ebcab7	3	// Copyright (c) 2008 - 2009, Apple Inc. All rights reserved.<BR>
2ef2b01e	4	//
d6ebcab7	5	// This program and the accompanying materials
2ef2b01e A	6	// are licensed and made available under the terms and conditions of the BSD License
	7	// which accompanies this distribution. The full text of the license may be found at
	8	// http://opensource.org/licenses/bsd-license.php
	9	//
	10	// THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
	11	// WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
	12	//
	13	//------------------------------------------------------------------------------
	14
	15	EXPORT ArmInvalidateInstructionCache
	16	EXPORT ArmInvalidateDataCacheEntryByMVA
	17	EXPORT ArmCleanDataCacheEntryByMVA
	18	EXPORT ArmCleanInvalidateDataCacheEntryByMVA
	19	EXPORT ArmInvalidateDataCacheEntryBySetWay
	20	EXPORT ArmCleanDataCacheEntryBySetWay
	21	EXPORT ArmCleanInvalidateDataCacheEntryBySetWay
	22	EXPORT ArmDrainWriteBuffer
	23	EXPORT ArmEnableMmu
	24	EXPORT ArmDisableMmu
1bfda055	25	EXPORT ArmDisableCachesAndMmu
c2b5ca8b	26	EXPORT ArmMmuEnabled
2ef2b01e A	27	EXPORT ArmEnableDataCache
	28	EXPORT ArmDisableDataCache
	29	EXPORT ArmEnableInstructionCache
	30	EXPORT ArmDisableInstructionCache
1bfda055	31	EXPORT ArmEnableSWPInstruction
2ef2b01e A	32	EXPORT ArmEnableBranchPrediction
2ef2b01e A	33	EXPORT ArmDisableBranchPrediction
37b91c49	34	EXPORT ArmV7AllDataCachesOperation
026c3d34	35	EXPORT ArmDataMemoryBarrier
	36	EXPORT ArmDataSyncronizationBarrier
	37	EXPORT ArmInstructionSynchronizationBarrier
1bfda055	38	EXPORT ArmWriteNsacr
	39	EXPORT ArmWriteScr
	40	EXPORT ArmWriteVMBar
	41	EXPORT ArmWriteVBar
	42	EXPORT ArmReadVBar
	43	EXPORT ArmWriteCPACR
	44	EXPORT ArmEnableVFP
	45	EXPORT ArmCallWFI
	46	EXPORT ArmWriteAuxCr
	47	EXPORT ArmReadAuxCr
	48	EXPORT ArmReadCbar
	49	EXPORT ArmInvalidateInstructionAndDataTlb
	50	EXPORT ArmReadMpidr
026c3d34	51
548af3e7	52	AREA ArmCacheLib, CODE, READONLY
548af3e7	53	PRESERVE8
2ef2b01e	54
1bfda055	55	DC_ON EQU ( 0x1:SHL:2 )
	56	IC_ON EQU ( 0x1:SHL:12 )
	57	CTRL_M_BIT EQU (1 << 0)
	58	CTRL_C_BIT EQU (1 << 2)
	59	CTRL_B_BIT EQU (1 << 7)
	60	CTRL_I_BIT EQU (1 << 12)
2ef2b01e	61
2ef2b01e A	62
2ef2b01e A	63	ArmInvalidateDataCacheEntryByMVA
548af3e7	64	mcr p15, 0, r0, c7, c6, 1 ; invalidate single data cache line
	65	dsb
	66	isb
	67	bx lr
2ef2b01e A	68
	69
	70	ArmCleanDataCacheEntryByMVA
548af3e7	71	mcr p15, 0, r0, c7, c10, 1 ; clean single data cache line
	72	dsb
	73	isb
	74	bx lr
2ef2b01e A	75
	76
	77	ArmCleanInvalidateDataCacheEntryByMVA
548af3e7	78	mcr p15, 0, r0, c7, c14, 1 ; clean and invalidate single data cache line
	79	dsb
	80	isb
	81	bx lr
2ef2b01e A	82
	83
	84	ArmInvalidateDataCacheEntryBySetWay
2ac288f9	85	mcr p15, 0, r0, c7, c6, 2 ; Invalidate this line
548af3e7	86	dsb
548af3e7	87	isb
2ef2b01e A	88	bx lr
	89
	90
	91	ArmCleanInvalidateDataCacheEntryBySetWay
2ac288f9	92	mcr p15, 0, r0, c7, c14, 2 ; Clean and Invalidate this line
548af3e7	93	dsb
548af3e7	94	isb
2ef2b01e A	95	bx lr
	96
	97
	98	ArmCleanDataCacheEntryBySetWay
2ac288f9	99	mcr p15, 0, r0, c7, c10, 2 ; Clean this line
548af3e7	100	dsb
548af3e7	101	isb
2ef2b01e A	102	bx lr
	103
	104
2ef2b01e	105	ArmInvalidateInstructionCache
548af3e7	106	mcr p15,0,R0,c7,c5,0 ;Invalidate entire instruction cache
548af3e7	107	isb
548af3e7	108	bx LR
2ef2b01e A	109
2ef2b01e A	110	ArmEnableMmu
1bfda055	111	mrc p15,0,R0,c1,c0,0 ; Read SCTLR into R0 (Read control register configuration data)
	112	orr R0,R0,#1 ; Set SCTLR.M bit : Enable MMU
	113	mcr p15,0,R0,c1,c0,0 ; Write R0 into SCTLR (Write control register configuration data)
548af3e7	114	dsb
548af3e7	115	isb
2ef2b01e A	116	bx LR
2ef2b01e A	117
c2b5ca8b	118	ArmMmuEnabled
1bfda055	119	mrc p15,0,R0,c1,c0,0 ; Read SCTLR into R0 (Read control register configuration data)
c2b5ca8b A	120	and R0,R0,#1
	121	bx LR
	122
2ef2b01e	123	ArmDisableMmu
1bfda055	124	mrc p15,0,R0,c1,c0,0 ; Read SCTLR into R0 (Read control register configuration data)
	125	bic R0,R0,#1 ; Clear SCTLR.M bit : Disable MMU
	126	mcr p15,0,R0,c1,c0,0 ; Write R0 into SCTLR (Write control register configuration data)
7800c283	127
2ac288f9	128	mcr p15,0,R0,c8,c7,0 ; TLBIALL : Invalidate unified TLB
1bfda055	129	mcr p15,0,R0,c7,c5,6 ; BPIALL : Invalidate entire branch predictor array
548af3e7	130	dsb
548af3e7	131	isb
2ef2b01e A	132	bx LR
2ef2b01e A	133
1bfda055	134	ArmDisableCachesAndMmu
	135	mrc p15, 0, r0, c1, c0, 0 ; Get control register
	136	bic r0, r0, #CTRL_M_BIT ; Disable MMU
	137	bic r0, r0, #CTRL_C_BIT ; Disable D Cache
	138	bic r0, r0, #CTRL_I_BIT ; Disable I Cache
	139	mcr p15, 0, r0, c1, c0, 0 ; Write control register
	140	dsb
	141	isb
	142	bx LR
bb02cb80	143
2ef2b01e	144	ArmEnableDataCache
1bfda055	145	ldr R1,=DC_ON ; Specify SCTLR.C bit : (Data) Cache enable bit
	146	mrc p15,0,R0,c1,c0,0 ; Read SCTLR into R0 (Read control register configuration data)
	147	orr R0,R0,R1 ; Set SCTLR.C bit : Data and unified caches enabled
	148	mcr p15,0,R0,c1,c0,0 ; Write R0 into SCTLR (Write control register configuration data)
548af3e7	149	dsb
	150	isb
	151	bx LR
2ef2b01e A	152
2ef2b01e A	153	ArmDisableDataCache
1bfda055	154	ldr R1,=DC_ON ; Specify SCTLR.C bit : (Data) Cache enable bit
	155	mrc p15,0,R0,c1,c0,0 ; Read SCTLR into R0 (Read control register configuration data)
	156	bic R0,R0,R1 ; Clear SCTLR.C bit : Data and unified caches disabled
	157	mcr p15,0,R0,c1,c0,0 ; Write R0 into SCTLR (Write control register configuration data)
548af3e7	158	isb
548af3e7	159	bx LR
2ef2b01e A	160
2ef2b01e A	161	ArmEnableInstructionCache
1bfda055	162	ldr R1,=IC_ON ; Specify SCTLR.I bit : Instruction cache enable bit
	163	mrc p15,0,R0,c1,c0,0 ; Read SCTLR into R0 (Read control register configuration data)
	164	orr R0,R0,R1 ; Set SCTLR.I bit : Instruction caches enabled
	165	mcr p15,0,R0,c1,c0,0 ; Write R0 into SCTLR (Write control register configuration data)
548af3e7	166	dsb
	167	isb
	168	bx LR
2ef2b01e A	169
2ef2b01e A	170	ArmDisableInstructionCache
1bfda055	171	ldr R1,=IC_ON ; Specify SCTLR.I bit : Instruction cache enable bit
	172	mrc p15,0,R0,c1,c0,0 ; Read SCTLR into R0 (Read control register configuration data)
	173	BIC R0,R0,R1 ; Clear SCTLR.I bit : Instruction caches disabled
	174	mcr p15,0,R0,c1,c0,0 ; Write R0 into SCTLR (Write control register configuration data)
548af3e7	175	isb
548af3e7	176	bx LR
2ef2b01e	177
1bfda055	178	ArmEnableSWPInstruction
2ef2b01e	179	mrc p15, 0, r0, c1, c0, 0
1bfda055	180	orr r0, r0, #0x00000400
2ef2b01e	181	mcr p15, 0, r0, c1, c0, 0
548af3e7	182	isb
2ef2b01e A	183	bx LR
2ef2b01e A	184
1bfda055	185	ArmEnableBranchPrediction
	186	mrc p15, 0, r0, c1, c0, 0 ; Read SCTLR into R0 (Read control register configuration data)
	187	orr r0, r0, #0x00000800 ;
	188	mcr p15, 0, r0, c1, c0, 0 ; Write R0 into SCTLR (Write control register configuration data)
	189	isb
	190	bx LR
	191
2ef2b01e	192	ArmDisableBranchPrediction
1bfda055	193	mrc p15, 0, r0, c1, c0, 0 ; Read SCTLR into R0 (Read control register configuration data)
	194	bic r0, r0, #0x00000800 ;
	195	mcr p15, 0, r0, c1, c0, 0 ; Write R0 into SCTLR (Write control register configuration data)
548af3e7	196	isb
2ef2b01e A	197	bx LR
2ef2b01e A	198
98bc0c8c	199
98bc0c8c	200	ArmV7AllDataCachesOperation
548af3e7	201	stmfd SP!,{r4-r12, LR}
	202	mov R1, R0 ; Save Function call in R1
	203	mrc p15, 1, R6, c0, c0, 1 ; Read CLIDR
	204	ands R3, R6, #&7000000 ; Mask out all but Level of Coherency (LoC)
	205	mov R3, R3, LSR #23 ; Cache level value (naturally aligned)
	206	beq Finished
	207	mov R10, #0
98bc0c8c	208
98bc0c8c	209	Loop1
1bfda055	210	add R2, R10, R10, LSR #1 ; Work out 3xcachelevel
	211	mov R12, R6, LSR R2 ; bottom 3 bits are the Cache type for this level
	212	and R12, R12, #7 ; get those 3 bits alone
548af3e7	213	cmp R12, #2
	214	blt Skip ; no cache or only instruction cache at this level
	215	mcr p15, 2, R10, c0, c0, 0 ; write the Cache Size selection register (CSSELR) // OR in 1 for Instruction
	216	isb ; isb to sync the change to the CacheSizeID reg
	217	mrc p15, 1, R12, c0, c0, 0 ; reads current Cache Size ID register (CCSIDR)
	218	and R2, R12, #&7 ; extract the line length field
	219	add R2, R2, #4 ; add 4 for the line length offset (log2 16 bytes)
	220	ldr R4, =0x3FF
	221	ands R4, R4, R12, LSR #3 ; R4 is the max number on the way size (right aligned)
	222	clz R5, R4 ; R5 is the bit position of the way size increment
	223	ldr R7, =0x00007FFF
	224	ands R7, R7, R12, LSR #13 ; R7 is the max number of the index size (right aligned)
98bc0c8c	225
98bc0c8c	226	Loop2
548af3e7	227	mov R9, R4 ; R9 working copy of the max way size (right aligned)
98bc0c8c	228
98bc0c8c	229	Loop3
548af3e7	230	orr R0, R10, R9, LSL R5 ; factor in the way number and cache number into R11
548af3e7	231	orr R0, R0, R7, LSL R2 ; factor in the index number
98bc0c8c	232
548af3e7	233	blx R1
98bc0c8c	234
548af3e7	235	subs R9, R9, #1 ; decrement the way number
	236	bge Loop3
	237	subs R7, R7, #1 ; decrement the index
	238	bge Loop2
98bc0c8c	239	Skip
548af3e7	240	add R10, R10, #2 ; increment the cache number
	241	cmp R3, R10
	242	bgt Loop1
98bc0c8c	243
98bc0c8c	244	Finished
7800c283	245	dsb
548af3e7	246	ldmfd SP!, {r4-r12, lr}
548af3e7	247	bx LR
98bc0c8c	248
026c3d34	249
026c3d34	250	ArmDataMemoryBarrier
548af3e7	251	dmb
548af3e7	252	bx LR
026c3d34	253
026c3d34	254	ArmDataSyncronizationBarrier
7800c283	255	ArmDrainWriteBuffer
548af3e7	256	dsb
548af3e7	257	bx LR
026c3d34	258
026c3d34	259	ArmInstructionSynchronizationBarrier
548af3e7	260	isb
548af3e7	261	bx LR
026c3d34	262
1bfda055	263	ArmWriteNsacr
	264	mcr p15, 0, r0, c1, c1, 2
	265	bx lr
	266
	267	ArmWriteScr
	268	mcr p15, 0, r0, c1, c1, 0
	269	bx lr
	270
	271	ArmWriteAuxCr
	272	mcr p15, 0, r0, c1, c0, 1
	273	bx lr
	274
	275	ArmReadAuxCr
	276	mrc p15, 0, r0, c1, c0, 1
	277	bx lr
	278
	279	ArmWriteVMBar
	280	mcr p15, 0, r0, c12, c0, 1
	281	bx lr
	282
	283	ArmWriteVBar
	284	mcr p15, 0, r0, c12, c0, 0
	285	bx lr
	286
	287	ArmReadVBar
	288	mrc p15, 0, r0, c12, c0, 0
	289	bx lr
	290
	291	ArmWriteCPACR
	292	mcr p15, 0, r0, c1, c0, 2
	293	bx lr
	294
	295	ArmEnableVFP
	296	// Enable VFP registers
	297	mrc p15, 0, r0, c1, c0, 2
	298	orr r0, r0, #0x00f00000 // Enable VPF access (V* instructions)
	299	mcr p15, 0, r0, c1, c0, 2
	300	mov r0, #0x40000000 // Set EN bit in FPEXC
	301	mcr p10,#0x7,r0,c8,c0,#0 // msr FPEXC,r0 in ARM assembly
	302	bx lr
	303
	304	ArmCallWFI
	305	wfi
	306	bx lr
	307
	308	//Note: Return 0 in Uniprocessor implementation
	309	ArmReadCbar
2ac288f9	310	mrc p15, 4, r0, c15, c0, 0 //Read Configuration Base Address Register
1bfda055	311	bx lr
	312
	313	ArmInvalidateInstructionAndDataTlb
	314	mcr p15, 0, r0, c8, c7, 0 ; Invalidate Inst TLB and Data TLB
	315	dsb
	316	bx lr
	317
	318	ArmReadMpidr
2ac288f9	319	mrc p15, 0, r0, c0, c0, 5 ; read MPIDR
1bfda055	320	bx lr
	321
	322	END
e9fc14b6	323