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

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